diff options
author | Campbell Barton <ideasman42@gmail.com> | 2019-04-17 07:17:24 +0300 |
---|---|---|
committer | Campbell Barton <ideasman42@gmail.com> | 2019-04-17 07:21:24 +0300 |
commit | e12c08e8d170b7ca40f204a5b0423c23a9fbc2c1 (patch) | |
tree | 8cf3453d12edb177a218ef8009357518ec6cab6a /source/blender/ikplugin | |
parent | b3dabc200a4b0399ec6b81f2ff2730d07b44fcaa (diff) |
ClangFormat: apply to source, most of intern
Apply clang format as proposed in T53211.
For details on usage and instructions for migrating branches
without conflicts, see:
https://wiki.blender.org/wiki/Tools/ClangFormat
Diffstat (limited to 'source/blender/ikplugin')
-rw-r--r-- | source/blender/ikplugin/BIK_api.h | 42 | ||||
-rw-r--r-- | source/blender/ikplugin/CMakeLists.txt | 66 | ||||
-rw-r--r-- | source/blender/ikplugin/intern/ikplugin_api.c | 98 | ||||
-rw-r--r-- | source/blender/ikplugin/intern/ikplugin_api.h | 25 | ||||
-rw-r--r-- | source/blender/ikplugin/intern/iksolver_plugin.c | 1016 | ||||
-rw-r--r-- | source/blender/ikplugin/intern/iksolver_plugin.h | 17 | ||||
-rw-r--r-- | source/blender/ikplugin/intern/itasc_plugin.cpp | 3358 | ||||
-rw-r--r-- | source/blender/ikplugin/intern/itasc_plugin.h | 16 |
8 files changed, 2389 insertions, 2249 deletions
diff --git a/source/blender/ikplugin/BIK_api.h b/source/blender/ikplugin/BIK_api.h index 77571dc631f..ade3113f041 100644 --- a/source/blender/ikplugin/BIK_api.h +++ b/source/blender/ikplugin/BIK_api.h @@ -22,7 +22,6 @@ * \ingroup ikplugin */ - #ifndef __BIK_API_H__ #define __BIK_API_H__ @@ -38,24 +37,31 @@ struct bPose; struct bPoseChannel; enum BIK_ParamType { - BIK_PARAM_TYPE_FLOAT = 0, - BIK_PARAM_TYPE_INT, - BIK_PARAM_TYPE_STRING, + BIK_PARAM_TYPE_FLOAT = 0, + BIK_PARAM_TYPE_INT, + BIK_PARAM_TYPE_STRING, }; struct BIK_ParamValue { - short type; /* BIK_PARAM_TYPE_.. */ - short length; /* for string, does not include terminating 0 */ - union { - float f[8]; - int i[8]; - char s[32]; - } value; + short type; /* BIK_PARAM_TYPE_.. */ + short length; /* for string, does not include terminating 0 */ + union { + float f[8]; + int i[8]; + char s[32]; + } value; }; typedef struct BIK_ParamValue BIK_ParamValue; -void BIK_initialize_tree(struct Depsgraph *depsgraph, struct Scene *scene, struct Object *ob, float ctime); -void BIK_execute_tree(struct Depsgraph *depsgraph, struct Scene *scene, struct Object *ob, struct bPoseChannel *pchan, float ctime); +void BIK_initialize_tree(struct Depsgraph *depsgraph, + struct Scene *scene, + struct Object *ob, + float ctime); +void BIK_execute_tree(struct Depsgraph *depsgraph, + struct Scene *scene, + struct Object *ob, + struct bPoseChannel *pchan, + float ctime); void BIK_release_tree(struct Scene *scene, struct Object *ob, float ctime); void BIK_clear_data(struct bPose *pose); void BIK_clear_cache(struct bPose *pose); @@ -71,17 +77,17 @@ void BIK_test_constraint(struct Object *ob, struct bConstraint *cons); // 1 = iTaSC /* for use in BIK_get_constraint_param */ -#define BIK_PARAM_CONSTRAINT_ERROR 0 +#define BIK_PARAM_CONSTRAINT_ERROR 0 /* for use in BIK_get_channel_param */ -#define BIK_PARAM_CHANNEL_JOINT 0 +#define BIK_PARAM_CHANNEL_JOINT 0 /* for use in BIK_get_solver_param */ -#define BIK_PARAM_SOLVER_RANK 0 -#define BIK_PARAM_SOLVER_ITERATION 1 +#define BIK_PARAM_SOLVER_RANK 0 +#define BIK_PARAM_SOLVER_ITERATION 1 #ifdef __cplusplus } #endif -#endif /* __BIK_API_H__ */ +#endif /* __BIK_API_H__ */ diff --git a/source/blender/ikplugin/CMakeLists.txt b/source/blender/ikplugin/CMakeLists.txt index 654d83d71af..1a0bcbb9dbc 100644 --- a/source/blender/ikplugin/CMakeLists.txt +++ b/source/blender/ikplugin/CMakeLists.txt @@ -21,11 +21,11 @@ remove_extra_strict_flags() set(INC - . - ../blenkernel - ../blenlib - ../makesdna - ../../../intern/guardedalloc + . + ../blenkernel + ../blenlib + ../makesdna + ../../../intern/guardedalloc ) set(INC_SYS @@ -33,44 +33,44 @@ set(INC_SYS ) set(SRC - intern/ikplugin_api.c + intern/ikplugin_api.c - BIK_api.h - intern/ikplugin_api.h + BIK_api.h + intern/ikplugin_api.h ) set(LIB ) if(WITH_IK_SOLVER) - list(APPEND LIB - bf_intern_iksolver - ) - list(APPEND INC - ../../../intern/iksolver/extern - ) - list(APPEND SRC - intern/iksolver_plugin.c - intern/iksolver_plugin.h - ) - add_definitions(-DWITH_IK_SOLVER) + list(APPEND LIB + bf_intern_iksolver + ) + list(APPEND INC + ../../../intern/iksolver/extern + ) + list(APPEND SRC + intern/iksolver_plugin.c + intern/iksolver_plugin.h + ) + add_definitions(-DWITH_IK_SOLVER) endif() if(WITH_IK_ITASC) - list(APPEND LIB - bf_intern_itasc - ) - list(APPEND INC - ../../../intern/itasc - ) - list(APPEND INC_SYS - ${EIGEN3_INCLUDE_DIRS} - ) - list(APPEND SRC - intern/itasc_plugin.cpp - intern/itasc_plugin.h - ) - add_definitions(-DWITH_IK_ITASC) + list(APPEND LIB + bf_intern_itasc + ) + list(APPEND INC + ../../../intern/itasc + ) + list(APPEND INC_SYS + ${EIGEN3_INCLUDE_DIRS} + ) + list(APPEND SRC + intern/itasc_plugin.cpp + intern/itasc_plugin.h + ) + add_definitions(-DWITH_IK_ITASC) endif() blender_add_lib(bf_ikplugin "${SRC}" "${INC}" "${INC_SYS}" "${LIB}") diff --git a/source/blender/ikplugin/intern/ikplugin_api.c b/source/blender/ikplugin/intern/ikplugin_api.c index 3b658a4f822..3a851df7058 100644 --- a/source/blender/ikplugin/intern/ikplugin_api.c +++ b/source/blender/ikplugin/intern/ikplugin_api.c @@ -40,98 +40,98 @@ static IKPlugin ikplugin_tab[] = { #ifdef WITH_IK_SOLVER - /* Legacy IK solver */ - { - iksolver_initialize_tree, - iksolver_execute_tree, - iksolver_release_tree, - iksolver_clear_data, - NULL, - NULL, - NULL, - }, + /* Legacy IK solver */ + { + iksolver_initialize_tree, + iksolver_execute_tree, + iksolver_release_tree, + iksolver_clear_data, + NULL, + NULL, + NULL, + }, #endif #ifdef WITH_IK_ITASC - /* iTaSC IK solver */ - { - itasc_initialize_tree, - itasc_execute_tree, - itasc_release_tree, - itasc_clear_data, - itasc_clear_cache, - itasc_update_param, - itasc_test_constraint, - }, + /* iTaSC IK solver */ + { + itasc_initialize_tree, + itasc_execute_tree, + itasc_release_tree, + itasc_clear_data, + itasc_clear_cache, + itasc_update_param, + itasc_test_constraint, + }, #endif - { NULL } -}; + {NULL}}; static IKPlugin *get_plugin(bPose *pose) { - if (!pose || pose->iksolver < 0 || pose->iksolver >= ((sizeof(ikplugin_tab) / sizeof(IKPlugin)) - 1)) - return NULL; + if (!pose || pose->iksolver < 0 || + pose->iksolver >= ((sizeof(ikplugin_tab) / sizeof(IKPlugin)) - 1)) + return NULL; - return &ikplugin_tab[pose->iksolver]; + return &ikplugin_tab[pose->iksolver]; } - /*----------------------------------------*/ -/* Plugin API */ +/* Plugin API */ void BIK_initialize_tree(struct Depsgraph *depsgraph, Scene *scene, Object *ob, float ctime) { - IKPlugin *plugin = get_plugin(ob->pose); + IKPlugin *plugin = get_plugin(ob->pose); - if (plugin && plugin->initialize_tree_func) - plugin->initialize_tree_func(depsgraph, scene, ob, ctime); + if (plugin && plugin->initialize_tree_func) + plugin->initialize_tree_func(depsgraph, scene, ob, ctime); } -void BIK_execute_tree(struct Depsgraph *depsgraph, struct Scene *scene, Object *ob, bPoseChannel *pchan, float ctime) +void BIK_execute_tree( + struct Depsgraph *depsgraph, struct Scene *scene, Object *ob, bPoseChannel *pchan, float ctime) { - IKPlugin *plugin = get_plugin(ob->pose); + IKPlugin *plugin = get_plugin(ob->pose); - if (plugin && plugin->execute_tree_func) - plugin->execute_tree_func(depsgraph, scene, ob, pchan, ctime); + if (plugin && plugin->execute_tree_func) + plugin->execute_tree_func(depsgraph, scene, ob, pchan, ctime); } void BIK_release_tree(struct Scene *scene, Object *ob, float ctime) { - IKPlugin *plugin = get_plugin(ob->pose); + IKPlugin *plugin = get_plugin(ob->pose); - if (plugin && plugin->release_tree_func) - plugin->release_tree_func(scene, ob, ctime); + if (plugin && plugin->release_tree_func) + plugin->release_tree_func(scene, ob, ctime); } void BIK_clear_data(struct bPose *pose) { - IKPlugin *plugin = get_plugin(pose); + IKPlugin *plugin = get_plugin(pose); - if (plugin && plugin->remove_armature_func) - plugin->remove_armature_func(pose); + if (plugin && plugin->remove_armature_func) + plugin->remove_armature_func(pose); } void BIK_clear_cache(struct bPose *pose) { - IKPlugin *plugin = get_plugin(pose); + IKPlugin *plugin = get_plugin(pose); - if (plugin && plugin->clear_cache) - plugin->clear_cache(pose); + if (plugin && plugin->clear_cache) + plugin->clear_cache(pose); } void BIK_update_param(struct bPose *pose) { - IKPlugin *plugin = get_plugin(pose); + IKPlugin *plugin = get_plugin(pose); - if (plugin && plugin->update_param) - plugin->update_param(pose); + if (plugin && plugin->update_param) + plugin->update_param(pose); } void BIK_test_constraint(struct Object *ob, struct bConstraint *cons) { - IKPlugin *plugin = get_plugin(ob->pose); + IKPlugin *plugin = get_plugin(ob->pose); - if (plugin && plugin->test_constraint) - plugin->test_constraint(ob, cons); + if (plugin && plugin->test_constraint) + plugin->test_constraint(ob, cons); } diff --git a/source/blender/ikplugin/intern/ikplugin_api.h b/source/blender/ikplugin/intern/ikplugin_api.h index a58fc4c4d25..faf21cecacd 100644 --- a/source/blender/ikplugin/intern/ikplugin_api.h +++ b/source/blender/ikplugin/intern/ikplugin_api.h @@ -22,7 +22,6 @@ * \ingroup ikplugin */ - #ifndef __IKPLUGIN_API_H__ #define __IKPLUGIN_API_H__ @@ -35,15 +34,21 @@ struct Object; struct Scene; struct bPoseChannel; - struct IKPlugin { - void (*initialize_tree_func)(struct Depsgraph *depsgraph, struct Scene *scene, struct Object *ob, float ctime); - void (*execute_tree_func)(struct Depsgraph *depsgraph, struct Scene *scene, struct Object *ob, struct bPoseChannel *pchan, float ctime); - void (*release_tree_func)(struct Scene *scene, struct Object *ob, float ctime); - void (*remove_armature_func)(struct bPose *pose); - void (*clear_cache)(struct bPose *pose); - void (*update_param)(struct bPose *pose); - void (*test_constraint)(struct Object *ob, struct bConstraint *cons); + void (*initialize_tree_func)(struct Depsgraph *depsgraph, + struct Scene *scene, + struct Object *ob, + float ctime); + void (*execute_tree_func)(struct Depsgraph *depsgraph, + struct Scene *scene, + struct Object *ob, + struct bPoseChannel *pchan, + float ctime); + void (*release_tree_func)(struct Scene *scene, struct Object *ob, float ctime); + void (*remove_armature_func)(struct bPose *pose); + void (*clear_cache)(struct bPose *pose); + void (*update_param)(struct bPose *pose); + void (*test_constraint)(struct Object *ob, struct bConstraint *cons); }; typedef struct IKPlugin IKPlugin; @@ -52,4 +57,4 @@ typedef struct IKPlugin IKPlugin; } #endif -#endif /* __IKPLUGIN_API_H__ */ +#endif /* __IKPLUGIN_API_H__ */ diff --git a/source/blender/ikplugin/intern/iksolver_plugin.c b/source/blender/ikplugin/intern/iksolver_plugin.c index ddb8b37afc2..5615cc4ea31 100644 --- a/source/blender/ikplugin/intern/iksolver_plugin.c +++ b/source/blender/ikplugin/intern/iksolver_plugin.c @@ -50,551 +50,575 @@ /* Note: detecting the IK chain is duplicate code... in drawarmature.c and in transform_conversions.c */ static void initialize_posetree(struct Object *UNUSED(ob), bPoseChannel *pchan_tip) { - bPoseChannel *curchan, *pchan_root = NULL, *chanlist[256], **oldchan; - PoseTree *tree; - PoseTarget *target; - bConstraint *con; - bKinematicConstraint *data; - int a, t, segcount = 0, size, newsize, *oldparent, parent; - - /* find IK constraint, and validate it */ - for (con = pchan_tip->constraints.first; con; con = con->next) { - if (con->type == CONSTRAINT_TYPE_KINEMATIC) { - data = (bKinematicConstraint *)con->data; - if (data->flag & CONSTRAINT_IK_AUTO) break; - if (data->tar == NULL) continue; - if (data->tar->type == OB_ARMATURE && data->subtarget[0] == 0) continue; - if ((con->flag & (CONSTRAINT_DISABLE | CONSTRAINT_OFF)) == 0 && (con->enforce != 0.0f)) break; - } - } - if (con == NULL) return; - - /* exclude tip from chain? */ - if (!(data->flag & CONSTRAINT_IK_TIP)) - pchan_tip = pchan_tip->parent; - - /* Find the chain's root & count the segments needed */ - for (curchan = pchan_tip; curchan; curchan = curchan->parent) { - pchan_root = curchan; - - curchan->flag |= POSE_CHAIN; // don't forget to clear this - chanlist[segcount] = curchan; - segcount++; - - if (segcount == data->rootbone || segcount > 255) break; // 255 is weak - } - if (!segcount) return; - - /* setup the chain data */ - - /* we make tree-IK, unless all existing targets are in this chain */ - for (tree = pchan_root->iktree.first; tree; tree = tree->next) { - for (target = tree->targets.first; target; target = target->next) { - curchan = tree->pchan[target->tip]; - if (curchan->flag & POSE_CHAIN) - curchan->flag &= ~POSE_CHAIN; - else - break; - } - if (target) break; - } - - /* create a target */ - target = MEM_callocN(sizeof(PoseTarget), "posetarget"); - target->con = con; - pchan_tip->flag &= ~POSE_CHAIN; - - if (tree == NULL) { - /* make new tree */ - tree = MEM_callocN(sizeof(PoseTree), "posetree"); - - tree->type = CONSTRAINT_TYPE_KINEMATIC; - - tree->iterations = data->iterations; - tree->totchannel = segcount; - tree->stretch = (data->flag & CONSTRAINT_IK_STRETCH); - - tree->pchan = MEM_callocN(segcount * sizeof(void *), "ik tree pchan"); - tree->parent = MEM_callocN(segcount * sizeof(int), "ik tree parent"); - for (a = 0; a < segcount; a++) { - tree->pchan[a] = chanlist[segcount - a - 1]; - tree->parent[a] = a - 1; - } - target->tip = segcount - 1; - - /* AND! link the tree to the root */ - BLI_addtail(&pchan_root->iktree, tree); - } - else { - tree->iterations = MAX2(data->iterations, tree->iterations); - tree->stretch = tree->stretch && !(data->flag & CONSTRAINT_IK_STRETCH); - - /* skip common pose channels and add remaining*/ - size = MIN2(segcount, tree->totchannel); - a = t = 0; - while (a < size && t < tree->totchannel) { - /* locate first matching channel */ - for (; t < tree->totchannel && tree->pchan[t] != chanlist[segcount - a - 1]; t++) ; - if (t >= tree->totchannel) - break; - for (; a < size && t < tree->totchannel && tree->pchan[t] == chanlist[segcount - a - 1]; a++, t++) ; - } - - segcount = segcount - a; - target->tip = tree->totchannel + segcount - 1; - - if (segcount > 0) { - for (parent = a - 1; parent < tree->totchannel; parent++) - if (tree->pchan[parent] == chanlist[segcount - 1]->parent) - break; - - /* shouldn't happen, but could with dependency cycles */ - if (parent == tree->totchannel) - parent = a - 1; - - /* resize array */ - newsize = tree->totchannel + segcount; - oldchan = tree->pchan; - oldparent = tree->parent; - - tree->pchan = MEM_callocN(newsize * sizeof(void *), "ik tree pchan"); - tree->parent = MEM_callocN(newsize * sizeof(int), "ik tree parent"); - memcpy(tree->pchan, oldchan, sizeof(void *) * tree->totchannel); - memcpy(tree->parent, oldparent, sizeof(int) * tree->totchannel); - MEM_freeN(oldchan); - MEM_freeN(oldparent); - - /* add new pose channels at the end, in reverse order */ - for (a = 0; a < segcount; a++) { - tree->pchan[tree->totchannel + a] = chanlist[segcount - a - 1]; - tree->parent[tree->totchannel + a] = tree->totchannel + a - 1; - } - tree->parent[tree->totchannel] = parent; - - tree->totchannel = newsize; - } - - /* move tree to end of list, for correct evaluation order */ - BLI_remlink(&pchan_root->iktree, tree); - BLI_addtail(&pchan_root->iktree, tree); - } - - /* add target to the tree */ - BLI_addtail(&tree->targets, target); - /* mark root channel having an IK tree */ - pchan_root->flag |= POSE_IKTREE; + bPoseChannel *curchan, *pchan_root = NULL, *chanlist[256], **oldchan; + PoseTree *tree; + PoseTarget *target; + bConstraint *con; + bKinematicConstraint *data; + int a, t, segcount = 0, size, newsize, *oldparent, parent; + + /* find IK constraint, and validate it */ + for (con = pchan_tip->constraints.first; con; con = con->next) { + if (con->type == CONSTRAINT_TYPE_KINEMATIC) { + data = (bKinematicConstraint *)con->data; + if (data->flag & CONSTRAINT_IK_AUTO) + break; + if (data->tar == NULL) + continue; + if (data->tar->type == OB_ARMATURE && data->subtarget[0] == 0) + continue; + if ((con->flag & (CONSTRAINT_DISABLE | CONSTRAINT_OFF)) == 0 && (con->enforce != 0.0f)) + break; + } + } + if (con == NULL) + return; + + /* exclude tip from chain? */ + if (!(data->flag & CONSTRAINT_IK_TIP)) + pchan_tip = pchan_tip->parent; + + /* Find the chain's root & count the segments needed */ + for (curchan = pchan_tip; curchan; curchan = curchan->parent) { + pchan_root = curchan; + + curchan->flag |= POSE_CHAIN; // don't forget to clear this + chanlist[segcount] = curchan; + segcount++; + + if (segcount == data->rootbone || segcount > 255) + break; // 255 is weak + } + if (!segcount) + return; + + /* setup the chain data */ + + /* we make tree-IK, unless all existing targets are in this chain */ + for (tree = pchan_root->iktree.first; tree; tree = tree->next) { + for (target = tree->targets.first; target; target = target->next) { + curchan = tree->pchan[target->tip]; + if (curchan->flag & POSE_CHAIN) + curchan->flag &= ~POSE_CHAIN; + else + break; + } + if (target) + break; + } + + /* create a target */ + target = MEM_callocN(sizeof(PoseTarget), "posetarget"); + target->con = con; + pchan_tip->flag &= ~POSE_CHAIN; + + if (tree == NULL) { + /* make new tree */ + tree = MEM_callocN(sizeof(PoseTree), "posetree"); + + tree->type = CONSTRAINT_TYPE_KINEMATIC; + + tree->iterations = data->iterations; + tree->totchannel = segcount; + tree->stretch = (data->flag & CONSTRAINT_IK_STRETCH); + + tree->pchan = MEM_callocN(segcount * sizeof(void *), "ik tree pchan"); + tree->parent = MEM_callocN(segcount * sizeof(int), "ik tree parent"); + for (a = 0; a < segcount; a++) { + tree->pchan[a] = chanlist[segcount - a - 1]; + tree->parent[a] = a - 1; + } + target->tip = segcount - 1; + + /* AND! link the tree to the root */ + BLI_addtail(&pchan_root->iktree, tree); + } + else { + tree->iterations = MAX2(data->iterations, tree->iterations); + tree->stretch = tree->stretch && !(data->flag & CONSTRAINT_IK_STRETCH); + + /* skip common pose channels and add remaining*/ + size = MIN2(segcount, tree->totchannel); + a = t = 0; + while (a < size && t < tree->totchannel) { + /* locate first matching channel */ + for (; t < tree->totchannel && tree->pchan[t] != chanlist[segcount - a - 1]; t++) + ; + if (t >= tree->totchannel) + break; + for (; a < size && t < tree->totchannel && tree->pchan[t] == chanlist[segcount - a - 1]; + a++, t++) + ; + } + + segcount = segcount - a; + target->tip = tree->totchannel + segcount - 1; + + if (segcount > 0) { + for (parent = a - 1; parent < tree->totchannel; parent++) + if (tree->pchan[parent] == chanlist[segcount - 1]->parent) + break; + + /* shouldn't happen, but could with dependency cycles */ + if (parent == tree->totchannel) + parent = a - 1; + + /* resize array */ + newsize = tree->totchannel + segcount; + oldchan = tree->pchan; + oldparent = tree->parent; + + tree->pchan = MEM_callocN(newsize * sizeof(void *), "ik tree pchan"); + tree->parent = MEM_callocN(newsize * sizeof(int), "ik tree parent"); + memcpy(tree->pchan, oldchan, sizeof(void *) * tree->totchannel); + memcpy(tree->parent, oldparent, sizeof(int) * tree->totchannel); + MEM_freeN(oldchan); + MEM_freeN(oldparent); + + /* add new pose channels at the end, in reverse order */ + for (a = 0; a < segcount; a++) { + tree->pchan[tree->totchannel + a] = chanlist[segcount - a - 1]; + tree->parent[tree->totchannel + a] = tree->totchannel + a - 1; + } + tree->parent[tree->totchannel] = parent; + + tree->totchannel = newsize; + } + + /* move tree to end of list, for correct evaluation order */ + BLI_remlink(&pchan_root->iktree, tree); + BLI_addtail(&pchan_root->iktree, tree); + } + + /* add target to the tree */ + BLI_addtail(&tree->targets, target); + /* mark root channel having an IK tree */ + pchan_root->flag |= POSE_IKTREE; } - /* transform from bone(b) to bone(b+1), store in chan_mat */ static void make_dmats(bPoseChannel *pchan) { - if (pchan->parent) { - float iR_parmat[4][4]; - invert_m4_m4(iR_parmat, pchan->parent->pose_mat); - mul_m4_m4m4(pchan->chan_mat, iR_parmat, pchan->pose_mat); // delta mat - } - else { - copy_m4_m4(pchan->chan_mat, pchan->pose_mat); - } + if (pchan->parent) { + float iR_parmat[4][4]; + invert_m4_m4(iR_parmat, pchan->parent->pose_mat); + mul_m4_m4m4(pchan->chan_mat, iR_parmat, pchan->pose_mat); // delta mat + } + else { + copy_m4_m4(pchan->chan_mat, pchan->pose_mat); + } } /* applies IK matrix to pchan, IK is done separated */ /* formula: pose_mat(b) = pose_mat(b-1) * diffmat(b-1, b) * ik_mat(b) */ /* to make this work, the diffmats have to be precalculated! Stored in chan_mat */ -static void where_is_ik_bone(bPoseChannel *pchan, float ik_mat[3][3]) // nr = to detect if this is first bone +static void where_is_ik_bone(bPoseChannel *pchan, + float ik_mat[3][3]) // nr = to detect if this is first bone { - float vec[3], ikmat[4][4]; + float vec[3], ikmat[4][4]; - copy_m4_m3(ikmat, ik_mat); + copy_m4_m3(ikmat, ik_mat); - if (pchan->parent) - mul_m4_m4m4(pchan->pose_mat, pchan->parent->pose_mat, pchan->chan_mat); - else - copy_m4_m4(pchan->pose_mat, pchan->chan_mat); + if (pchan->parent) + mul_m4_m4m4(pchan->pose_mat, pchan->parent->pose_mat, pchan->chan_mat); + else + copy_m4_m4(pchan->pose_mat, pchan->chan_mat); #ifdef USE_NONUNIFORM_SCALE - /* apply IK mat, but as if the bones have uniform scale since the IK solver - * is not aware of non-uniform scale */ - float scale[3]; - mat4_to_size(scale, pchan->pose_mat); - normalize_v3_length(pchan->pose_mat[0], scale[1]); - normalize_v3_length(pchan->pose_mat[2], scale[1]); + /* apply IK mat, but as if the bones have uniform scale since the IK solver + * is not aware of non-uniform scale */ + float scale[3]; + mat4_to_size(scale, pchan->pose_mat); + normalize_v3_length(pchan->pose_mat[0], scale[1]); + normalize_v3_length(pchan->pose_mat[2], scale[1]); #endif - mul_m4_m4m4(pchan->pose_mat, pchan->pose_mat, ikmat); + mul_m4_m4m4(pchan->pose_mat, pchan->pose_mat, ikmat); #ifdef USE_NONUNIFORM_SCALE - float ik_scale[3]; - mat3_to_size(ik_scale, ik_mat); - normalize_v3_length(pchan->pose_mat[0], scale[0] * ik_scale[0]); - normalize_v3_length(pchan->pose_mat[2], scale[2] * ik_scale[2]); + float ik_scale[3]; + mat3_to_size(ik_scale, ik_mat); + normalize_v3_length(pchan->pose_mat[0], scale[0] * ik_scale[0]); + normalize_v3_length(pchan->pose_mat[2], scale[2] * ik_scale[2]); #endif - /* calculate head */ - copy_v3_v3(pchan->pose_head, pchan->pose_mat[3]); - /* calculate tail */ - copy_v3_v3(vec, pchan->pose_mat[1]); - mul_v3_fl(vec, pchan->bone->length); - add_v3_v3v3(pchan->pose_tail, pchan->pose_head, vec); + /* calculate head */ + copy_v3_v3(pchan->pose_head, pchan->pose_mat[3]); + /* calculate tail */ + copy_v3_v3(vec, pchan->pose_mat[1]); + mul_v3_fl(vec, pchan->bone->length); + add_v3_v3v3(pchan->pose_tail, pchan->pose_head, vec); - pchan->flag |= POSE_DONE; + pchan->flag |= POSE_DONE; } - /* called from within the core BKE_pose_where_is loop, all animsystems and constraints * were executed & assigned. Now as last we do an IK pass */ -static void execute_posetree(struct Depsgraph *depsgraph, struct Scene *scene, Object *ob, PoseTree *tree) +static void execute_posetree(struct Depsgraph *depsgraph, + struct Scene *scene, + Object *ob, + PoseTree *tree) { - float R_parmat[3][3], identity[3][3]; - float iR_parmat[3][3]; - float R_bonemat[3][3]; - float goalrot[3][3], goalpos[3]; - float rootmat[4][4], imat[4][4]; - float goal[4][4], goalinv[4][4]; - float irest_basis[3][3], full_basis[3][3]; - float end_pose[4][4], world_pose[4][4]; - float basis[3][3], rest_basis[3][3], start[3], *ikstretch = NULL; - float resultinf = 0.0f; - int a, flag, hasstretch = 0, resultblend = 0; - bPoseChannel *pchan; - IK_Segment *seg, *parent, **iktree, *iktarget; - IK_Solver *solver; - PoseTarget *target; - bKinematicConstraint *data, *poleangledata = NULL; - Bone *bone; - - if (tree->totchannel == 0) - return; - - iktree = MEM_mallocN(sizeof(void *) * tree->totchannel, "ik tree"); - - for (a = 0; a < tree->totchannel; a++) { - float length; - pchan = tree->pchan[a]; - bone = pchan->bone; - - /* set DoF flag */ - flag = 0; - if (!(pchan->ikflag & BONE_IK_NO_XDOF) && !(pchan->ikflag & BONE_IK_NO_XDOF_TEMP)) - flag |= IK_XDOF; - if (!(pchan->ikflag & BONE_IK_NO_YDOF) && !(pchan->ikflag & BONE_IK_NO_YDOF_TEMP)) - flag |= IK_YDOF; - if (!(pchan->ikflag & BONE_IK_NO_ZDOF) && !(pchan->ikflag & BONE_IK_NO_ZDOF_TEMP)) - flag |= IK_ZDOF; - - if (tree->stretch && (pchan->ikstretch > 0.0f)) { - flag |= IK_TRANS_YDOF; - hasstretch = 1; - } - - seg = iktree[a] = IK_CreateSegment(flag); - - /* find parent */ - if (a == 0) - parent = NULL; - else - parent = iktree[tree->parent[a]]; - - IK_SetParent(seg, parent); - - /* get the matrix that transforms from prevbone into this bone */ - copy_m3_m4(R_bonemat, pchan->pose_mat); - - /* gather transformations for this IK segment */ - - if (pchan->parent) - copy_m3_m4(R_parmat, pchan->parent->pose_mat); - else - unit_m3(R_parmat); - - /* bone offset */ - if (pchan->parent && (a > 0)) - sub_v3_v3v3(start, pchan->pose_head, pchan->parent->pose_tail); - else - /* only root bone (a = 0) has no parent */ - start[0] = start[1] = start[2] = 0.0f; - - /* change length based on bone size */ - length = bone->length * len_v3(R_bonemat[1]); - - /* basis must be pure rotation */ - normalize_m3(R_bonemat); - normalize_m3(R_parmat); - - /* compute rest basis and its inverse */ - copy_m3_m3(rest_basis, bone->bone_mat); - transpose_m3_m3(irest_basis, bone->bone_mat); - - /* compute basis with rest_basis removed */ - invert_m3_m3(iR_parmat, R_parmat); - mul_m3_m3m3(full_basis, iR_parmat, R_bonemat); - mul_m3_m3m3(basis, irest_basis, full_basis); - - /* transform offset into local bone space */ - mul_m3_v3(iR_parmat, start); - - IK_SetTransform(seg, start, rest_basis, basis, length); - - if (pchan->ikflag & BONE_IK_XLIMIT) - IK_SetLimit(seg, IK_X, pchan->limitmin[0], pchan->limitmax[0]); - if (pchan->ikflag & BONE_IK_YLIMIT) - IK_SetLimit(seg, IK_Y, pchan->limitmin[1], pchan->limitmax[1]); - if (pchan->ikflag & BONE_IK_ZLIMIT) - IK_SetLimit(seg, IK_Z, pchan->limitmin[2], pchan->limitmax[2]); - - IK_SetStiffness(seg, IK_X, pchan->stiffness[0]); - IK_SetStiffness(seg, IK_Y, pchan->stiffness[1]); - IK_SetStiffness(seg, IK_Z, pchan->stiffness[2]); - - if (tree->stretch && (pchan->ikstretch > 0.0f)) { - const float ikstretch_sq = SQUARE(pchan->ikstretch); - /* this function does its own clamping */ - IK_SetStiffness(seg, IK_TRANS_Y, 1.0f - ikstretch_sq); - IK_SetLimit(seg, IK_TRANS_Y, IK_STRETCH_STIFF_MIN, IK_STRETCH_STIFF_MAX); - } - } - - solver = IK_CreateSolver(iktree[0]); - - /* set solver goals */ - - /* first set the goal inverse transform, assuming the root of tree was done ok! */ - pchan = tree->pchan[0]; - if (pchan->parent) { - /* transform goal by parent mat, so this rotation is not part of the - * segment's basis. otherwise rotation limits do not work on the - * local transform of the segment itself. */ - copy_m4_m4(rootmat, pchan->parent->pose_mat); - /* However, we do not want to get (i.e. reverse) parent's scale, as it generates [#31008] - * kind of nasty bugs... */ - normalize_m4(rootmat); - } - else - unit_m4(rootmat); - copy_v3_v3(rootmat[3], pchan->pose_head); - - mul_m4_m4m4(imat, ob->obmat, rootmat); - invert_m4_m4(goalinv, imat); - - for (target = tree->targets.first; target; target = target->next) { - float polepos[3]; - int poleconstrain = 0; - - data = (bKinematicConstraint *)target->con->data; - - /* 1.0=ctime, we pass on object for auto-ik (owner-type here is object, even though - * strictly speaking, it is a posechannel) - */ - BKE_constraint_target_matrix_get(depsgraph, scene, target->con, 0, CONSTRAINT_OBTYPE_OBJECT, ob, rootmat, 1.0); - - /* and set and transform goal */ - mul_m4_m4m4(goal, goalinv, rootmat); - - copy_v3_v3(goalpos, goal[3]); - copy_m3_m4(goalrot, goal); - normalize_m3(goalrot); - - /* same for pole vector target */ - if (data->poletar) { - BKE_constraint_target_matrix_get(depsgraph, scene, target->con, 1, CONSTRAINT_OBTYPE_OBJECT, ob, rootmat, 1.0); - - if (data->flag & CONSTRAINT_IK_SETANGLE) { - /* don't solve IK when we are setting the pole angle */ - break; - } - else { - mul_m4_m4m4(goal, goalinv, rootmat); - copy_v3_v3(polepos, goal[3]); - poleconstrain = 1; - - /* for pole targets, we blend the result of the ik solver - * instead of the target position, otherwise we can't get - * a smooth transition */ - resultblend = 1; - resultinf = target->con->enforce; - - if (data->flag & CONSTRAINT_IK_GETANGLE) { - poleangledata = data; - data->flag &= ~CONSTRAINT_IK_GETANGLE; - } - } - } - - /* do we need blending? */ - if (!resultblend && target->con->enforce != 1.0f) { - float q1[4], q2[4], q[4]; - float fac = target->con->enforce; - float mfac = 1.0f - fac; - - pchan = tree->pchan[target->tip]; - - /* end effector in world space */ - copy_m4_m4(end_pose, pchan->pose_mat); - copy_v3_v3(end_pose[3], pchan->pose_tail); - mul_m4_series(world_pose, goalinv, ob->obmat, end_pose); - - /* blend position */ - goalpos[0] = fac * goalpos[0] + mfac * world_pose[3][0]; - goalpos[1] = fac * goalpos[1] + mfac * world_pose[3][1]; - goalpos[2] = fac * goalpos[2] + mfac * world_pose[3][2]; - - /* blend rotation */ - mat3_to_quat(q1, goalrot); - mat4_to_quat(q2, world_pose); - interp_qt_qtqt(q, q1, q2, mfac); - quat_to_mat3(goalrot, q); - } - - iktarget = iktree[target->tip]; - - if ((data->flag & CONSTRAINT_IK_POS) && data->weight != 0.0f) { - if (poleconstrain) - IK_SolverSetPoleVectorConstraint(solver, iktarget, goalpos, - polepos, data->poleangle, (poleangledata == data)); - IK_SolverAddGoal(solver, iktarget, goalpos, data->weight); - } - if ((data->flag & CONSTRAINT_IK_ROT) && (data->orientweight != 0.0f)) - if ((data->flag & CONSTRAINT_IK_AUTO) == 0) - IK_SolverAddGoalOrientation(solver, iktarget, goalrot, - data->orientweight); - } - - /* solve */ - IK_Solve(solver, 0.0f, tree->iterations); - - if (poleangledata) - poleangledata->poleangle = IK_SolverGetPoleAngle(solver); - - IK_FreeSolver(solver); - - /* gather basis changes */ - tree->basis_change = MEM_mallocN(sizeof(float[3][3]) * tree->totchannel, "ik basis change"); - if (hasstretch) - ikstretch = MEM_mallocN(sizeof(float) * tree->totchannel, "ik stretch"); - - for (a = 0; a < tree->totchannel; a++) { - IK_GetBasisChange(iktree[a], tree->basis_change[a]); - - if (hasstretch) { - /* have to compensate for scaling received from parent */ - float parentstretch, stretch; - - pchan = tree->pchan[a]; - parentstretch = (tree->parent[a] >= 0) ? ikstretch[tree->parent[a]] : 1.0f; - - if (tree->stretch && (pchan->ikstretch > 0.0f)) { - float trans[3], length; - - IK_GetTranslationChange(iktree[a], trans); - length = pchan->bone->length * len_v3(pchan->pose_mat[1]); - - ikstretch[a] = (length == 0.0f) ? 1.0f : (trans[1] + length) / length; - } - else - ikstretch[a] = 1.0; - - stretch = (parentstretch == 0.0f) ? 1.0f : ikstretch[a] / parentstretch; - - mul_v3_fl(tree->basis_change[a][0], stretch); - mul_v3_fl(tree->basis_change[a][1], stretch); - mul_v3_fl(tree->basis_change[a][2], stretch); - } - - if (resultblend && resultinf != 1.0f) { - unit_m3(identity); - blend_m3_m3m3(tree->basis_change[a], identity, - tree->basis_change[a], resultinf); - } - - IK_FreeSegment(iktree[a]); - } - - MEM_freeN(iktree); - if (ikstretch) MEM_freeN(ikstretch); + float R_parmat[3][3], identity[3][3]; + float iR_parmat[3][3]; + float R_bonemat[3][3]; + float goalrot[3][3], goalpos[3]; + float rootmat[4][4], imat[4][4]; + float goal[4][4], goalinv[4][4]; + float irest_basis[3][3], full_basis[3][3]; + float end_pose[4][4], world_pose[4][4]; + float basis[3][3], rest_basis[3][3], start[3], *ikstretch = NULL; + float resultinf = 0.0f; + int a, flag, hasstretch = 0, resultblend = 0; + bPoseChannel *pchan; + IK_Segment *seg, *parent, **iktree, *iktarget; + IK_Solver *solver; + PoseTarget *target; + bKinematicConstraint *data, *poleangledata = NULL; + Bone *bone; + + if (tree->totchannel == 0) + return; + + iktree = MEM_mallocN(sizeof(void *) * tree->totchannel, "ik tree"); + + for (a = 0; a < tree->totchannel; a++) { + float length; + pchan = tree->pchan[a]; + bone = pchan->bone; + + /* set DoF flag */ + flag = 0; + if (!(pchan->ikflag & BONE_IK_NO_XDOF) && !(pchan->ikflag & BONE_IK_NO_XDOF_TEMP)) + flag |= IK_XDOF; + if (!(pchan->ikflag & BONE_IK_NO_YDOF) && !(pchan->ikflag & BONE_IK_NO_YDOF_TEMP)) + flag |= IK_YDOF; + if (!(pchan->ikflag & BONE_IK_NO_ZDOF) && !(pchan->ikflag & BONE_IK_NO_ZDOF_TEMP)) + flag |= IK_ZDOF; + + if (tree->stretch && (pchan->ikstretch > 0.0f)) { + flag |= IK_TRANS_YDOF; + hasstretch = 1; + } + + seg = iktree[a] = IK_CreateSegment(flag); + + /* find parent */ + if (a == 0) + parent = NULL; + else + parent = iktree[tree->parent[a]]; + + IK_SetParent(seg, parent); + + /* get the matrix that transforms from prevbone into this bone */ + copy_m3_m4(R_bonemat, pchan->pose_mat); + + /* gather transformations for this IK segment */ + + if (pchan->parent) + copy_m3_m4(R_parmat, pchan->parent->pose_mat); + else + unit_m3(R_parmat); + + /* bone offset */ + if (pchan->parent && (a > 0)) + sub_v3_v3v3(start, pchan->pose_head, pchan->parent->pose_tail); + else + /* only root bone (a = 0) has no parent */ + start[0] = start[1] = start[2] = 0.0f; + + /* change length based on bone size */ + length = bone->length * len_v3(R_bonemat[1]); + + /* basis must be pure rotation */ + normalize_m3(R_bonemat); + normalize_m3(R_parmat); + + /* compute rest basis and its inverse */ + copy_m3_m3(rest_basis, bone->bone_mat); + transpose_m3_m3(irest_basis, bone->bone_mat); + + /* compute basis with rest_basis removed */ + invert_m3_m3(iR_parmat, R_parmat); + mul_m3_m3m3(full_basis, iR_parmat, R_bonemat); + mul_m3_m3m3(basis, irest_basis, full_basis); + + /* transform offset into local bone space */ + mul_m3_v3(iR_parmat, start); + + IK_SetTransform(seg, start, rest_basis, basis, length); + + if (pchan->ikflag & BONE_IK_XLIMIT) + IK_SetLimit(seg, IK_X, pchan->limitmin[0], pchan->limitmax[0]); + if (pchan->ikflag & BONE_IK_YLIMIT) + IK_SetLimit(seg, IK_Y, pchan->limitmin[1], pchan->limitmax[1]); + if (pchan->ikflag & BONE_IK_ZLIMIT) + IK_SetLimit(seg, IK_Z, pchan->limitmin[2], pchan->limitmax[2]); + + IK_SetStiffness(seg, IK_X, pchan->stiffness[0]); + IK_SetStiffness(seg, IK_Y, pchan->stiffness[1]); + IK_SetStiffness(seg, IK_Z, pchan->stiffness[2]); + + if (tree->stretch && (pchan->ikstretch > 0.0f)) { + const float ikstretch_sq = SQUARE(pchan->ikstretch); + /* this function does its own clamping */ + IK_SetStiffness(seg, IK_TRANS_Y, 1.0f - ikstretch_sq); + IK_SetLimit(seg, IK_TRANS_Y, IK_STRETCH_STIFF_MIN, IK_STRETCH_STIFF_MAX); + } + } + + solver = IK_CreateSolver(iktree[0]); + + /* set solver goals */ + + /* first set the goal inverse transform, assuming the root of tree was done ok! */ + pchan = tree->pchan[0]; + if (pchan->parent) { + /* transform goal by parent mat, so this rotation is not part of the + * segment's basis. otherwise rotation limits do not work on the + * local transform of the segment itself. */ + copy_m4_m4(rootmat, pchan->parent->pose_mat); + /* However, we do not want to get (i.e. reverse) parent's scale, as it generates [#31008] + * kind of nasty bugs... */ + normalize_m4(rootmat); + } + else + unit_m4(rootmat); + copy_v3_v3(rootmat[3], pchan->pose_head); + + mul_m4_m4m4(imat, ob->obmat, rootmat); + invert_m4_m4(goalinv, imat); + + for (target = tree->targets.first; target; target = target->next) { + float polepos[3]; + int poleconstrain = 0; + + data = (bKinematicConstraint *)target->con->data; + + /* 1.0=ctime, we pass on object for auto-ik (owner-type here is object, even though + * strictly speaking, it is a posechannel) + */ + BKE_constraint_target_matrix_get( + depsgraph, scene, target->con, 0, CONSTRAINT_OBTYPE_OBJECT, ob, rootmat, 1.0); + + /* and set and transform goal */ + mul_m4_m4m4(goal, goalinv, rootmat); + + copy_v3_v3(goalpos, goal[3]); + copy_m3_m4(goalrot, goal); + normalize_m3(goalrot); + + /* same for pole vector target */ + if (data->poletar) { + BKE_constraint_target_matrix_get( + depsgraph, scene, target->con, 1, CONSTRAINT_OBTYPE_OBJECT, ob, rootmat, 1.0); + + if (data->flag & CONSTRAINT_IK_SETANGLE) { + /* don't solve IK when we are setting the pole angle */ + break; + } + else { + mul_m4_m4m4(goal, goalinv, rootmat); + copy_v3_v3(polepos, goal[3]); + poleconstrain = 1; + + /* for pole targets, we blend the result of the ik solver + * instead of the target position, otherwise we can't get + * a smooth transition */ + resultblend = 1; + resultinf = target->con->enforce; + + if (data->flag & CONSTRAINT_IK_GETANGLE) { + poleangledata = data; + data->flag &= ~CONSTRAINT_IK_GETANGLE; + } + } + } + + /* do we need blending? */ + if (!resultblend && target->con->enforce != 1.0f) { + float q1[4], q2[4], q[4]; + float fac = target->con->enforce; + float mfac = 1.0f - fac; + + pchan = tree->pchan[target->tip]; + + /* end effector in world space */ + copy_m4_m4(end_pose, pchan->pose_mat); + copy_v3_v3(end_pose[3], pchan->pose_tail); + mul_m4_series(world_pose, goalinv, ob->obmat, end_pose); + + /* blend position */ + goalpos[0] = fac * goalpos[0] + mfac * world_pose[3][0]; + goalpos[1] = fac * goalpos[1] + mfac * world_pose[3][1]; + goalpos[2] = fac * goalpos[2] + mfac * world_pose[3][2]; + + /* blend rotation */ + mat3_to_quat(q1, goalrot); + mat4_to_quat(q2, world_pose); + interp_qt_qtqt(q, q1, q2, mfac); + quat_to_mat3(goalrot, q); + } + + iktarget = iktree[target->tip]; + + if ((data->flag & CONSTRAINT_IK_POS) && data->weight != 0.0f) { + if (poleconstrain) + IK_SolverSetPoleVectorConstraint( + solver, iktarget, goalpos, polepos, data->poleangle, (poleangledata == data)); + IK_SolverAddGoal(solver, iktarget, goalpos, data->weight); + } + if ((data->flag & CONSTRAINT_IK_ROT) && (data->orientweight != 0.0f)) + if ((data->flag & CONSTRAINT_IK_AUTO) == 0) + IK_SolverAddGoalOrientation(solver, iktarget, goalrot, data->orientweight); + } + + /* solve */ + IK_Solve(solver, 0.0f, tree->iterations); + + if (poleangledata) + poleangledata->poleangle = IK_SolverGetPoleAngle(solver); + + IK_FreeSolver(solver); + + /* gather basis changes */ + tree->basis_change = MEM_mallocN(sizeof(float[3][3]) * tree->totchannel, "ik basis change"); + if (hasstretch) + ikstretch = MEM_mallocN(sizeof(float) * tree->totchannel, "ik stretch"); + + for (a = 0; a < tree->totchannel; a++) { + IK_GetBasisChange(iktree[a], tree->basis_change[a]); + + if (hasstretch) { + /* have to compensate for scaling received from parent */ + float parentstretch, stretch; + + pchan = tree->pchan[a]; + parentstretch = (tree->parent[a] >= 0) ? ikstretch[tree->parent[a]] : 1.0f; + + if (tree->stretch && (pchan->ikstretch > 0.0f)) { + float trans[3], length; + + IK_GetTranslationChange(iktree[a], trans); + length = pchan->bone->length * len_v3(pchan->pose_mat[1]); + + ikstretch[a] = (length == 0.0f) ? 1.0f : (trans[1] + length) / length; + } + else + ikstretch[a] = 1.0; + + stretch = (parentstretch == 0.0f) ? 1.0f : ikstretch[a] / parentstretch; + + mul_v3_fl(tree->basis_change[a][0], stretch); + mul_v3_fl(tree->basis_change[a][1], stretch); + mul_v3_fl(tree->basis_change[a][2], stretch); + } + + if (resultblend && resultinf != 1.0f) { + unit_m3(identity); + blend_m3_m3m3(tree->basis_change[a], identity, tree->basis_change[a], resultinf); + } + + IK_FreeSegment(iktree[a]); + } + + MEM_freeN(iktree); + if (ikstretch) + MEM_freeN(ikstretch); } static void free_posetree(PoseTree *tree) { - BLI_freelistN(&tree->targets); - if (tree->pchan) MEM_freeN(tree->pchan); - if (tree->parent) MEM_freeN(tree->parent); - if (tree->basis_change) MEM_freeN(tree->basis_change); - MEM_freeN(tree); + BLI_freelistN(&tree->targets); + if (tree->pchan) + MEM_freeN(tree->pchan); + if (tree->parent) + MEM_freeN(tree->parent); + if (tree->basis_change) + MEM_freeN(tree->basis_change); + MEM_freeN(tree); } ///---------------------------------------- /// Plugin API for legacy iksolver -void iksolver_initialize_tree(struct Depsgraph *UNUSED(depsgraph), struct Scene *UNUSED(scene), struct Object *ob, float UNUSED(ctime)) +void iksolver_initialize_tree(struct Depsgraph *UNUSED(depsgraph), + struct Scene *UNUSED(scene), + struct Object *ob, + float UNUSED(ctime)) { - bPoseChannel *pchan; + bPoseChannel *pchan; - for (pchan = ob->pose->chanbase.first; pchan; pchan = pchan->next) { - if (pchan->constflag & PCHAN_HAS_IK) // flag is set on editing constraints - initialize_posetree(ob, pchan); // will attach it to root! - } - ob->pose->flag &= ~POSE_WAS_REBUILT; + for (pchan = ob->pose->chanbase.first; pchan; pchan = pchan->next) { + if (pchan->constflag & PCHAN_HAS_IK) // flag is set on editing constraints + initialize_posetree(ob, pchan); // will attach it to root! + } + ob->pose->flag &= ~POSE_WAS_REBUILT; } -void iksolver_execute_tree(struct Depsgraph *depsgraph, struct Scene *scene, Object *ob, bPoseChannel *pchan_root, float ctime) +void iksolver_execute_tree(struct Depsgraph *depsgraph, + struct Scene *scene, + Object *ob, + bPoseChannel *pchan_root, + float ctime) { - while (pchan_root->iktree.first) { - PoseTree *tree = pchan_root->iktree.first; - int a; - - /* stop on the first tree that isn't a standard IK chain */ - if (tree->type != CONSTRAINT_TYPE_KINEMATIC) - return; - - /* 4. walk over the tree for regular solving */ - for (a = 0; a < tree->totchannel; a++) { - if (!(tree->pchan[a]->flag & POSE_DONE)) // successive trees can set the flag - BKE_pose_where_is_bone(depsgraph, scene, ob, tree->pchan[a], ctime, 1); - /* tell blender that this channel was controlled by IK, it's cleared on each BKE_pose_where_is() */ - tree->pchan[a]->flag |= POSE_CHAIN; - } - - /* 5. execute the IK solver */ - execute_posetree(depsgraph, scene, ob, tree); - - /* 6. apply the differences to the channels, - * we need to calculate the original differences first */ - for (a = 0; a < tree->totchannel; a++) { - make_dmats(tree->pchan[a]); - } - - for (a = 0; a < tree->totchannel; a++) { - /* sets POSE_DONE */ - where_is_ik_bone(tree->pchan[a], tree->basis_change[a]); - } - - /* 7. and free */ - BLI_remlink(&pchan_root->iktree, tree); - free_posetree(tree); - } + while (pchan_root->iktree.first) { + PoseTree *tree = pchan_root->iktree.first; + int a; + + /* stop on the first tree that isn't a standard IK chain */ + if (tree->type != CONSTRAINT_TYPE_KINEMATIC) + return; + + /* 4. walk over the tree for regular solving */ + for (a = 0; a < tree->totchannel; a++) { + if (!(tree->pchan[a]->flag & POSE_DONE)) // successive trees can set the flag + BKE_pose_where_is_bone(depsgraph, scene, ob, tree->pchan[a], ctime, 1); + /* tell blender that this channel was controlled by IK, it's cleared on each BKE_pose_where_is() */ + tree->pchan[a]->flag |= POSE_CHAIN; + } + + /* 5. execute the IK solver */ + execute_posetree(depsgraph, scene, ob, tree); + + /* 6. apply the differences to the channels, + * we need to calculate the original differences first */ + for (a = 0; a < tree->totchannel; a++) { + make_dmats(tree->pchan[a]); + } + + for (a = 0; a < tree->totchannel; a++) { + /* sets POSE_DONE */ + where_is_ik_bone(tree->pchan[a], tree->basis_change[a]); + } + + /* 7. and free */ + BLI_remlink(&pchan_root->iktree, tree); + free_posetree(tree); + } } -void iksolver_release_tree(struct Scene *UNUSED(scene), struct Object *ob, float UNUSED(ctime)) +void iksolver_release_tree(struct Scene *UNUSED(scene), struct Object *ob, float UNUSED(ctime)) { - iksolver_clear_data(ob->pose); + iksolver_clear_data(ob->pose); } void iksolver_clear_data(bPose *pose) { - for (bPoseChannel *pchan = pose->chanbase.first; pchan; pchan = pchan->next) { - if ((pchan->flag & POSE_IKTREE) == 0) - continue; + for (bPoseChannel *pchan = pose->chanbase.first; pchan; pchan = pchan->next) { + if ((pchan->flag & POSE_IKTREE) == 0) + continue; - while (pchan->iktree.first) { - PoseTree *tree = pchan->iktree.first; + while (pchan->iktree.first) { + PoseTree *tree = pchan->iktree.first; - /* stop on the first tree that isn't a standard IK chain */ - if (tree->type != CONSTRAINT_TYPE_KINEMATIC) - break; + /* stop on the first tree that isn't a standard IK chain */ + if (tree->type != CONSTRAINT_TYPE_KINEMATIC) + break; - BLI_remlink(&pchan->iktree, tree); - free_posetree(tree); - } - } + BLI_remlink(&pchan->iktree, tree); + free_posetree(tree); + } + } } diff --git a/source/blender/ikplugin/intern/iksolver_plugin.h b/source/blender/ikplugin/intern/iksolver_plugin.h index 63719440372..20a9e78cc47 100644 --- a/source/blender/ikplugin/intern/iksolver_plugin.h +++ b/source/blender/ikplugin/intern/iksolver_plugin.h @@ -22,7 +22,6 @@ * \ingroup ikplugin */ - #ifndef __IKSOLVER_PLUGIN_H__ #define __IKSOLVER_PLUGIN_H__ @@ -32,11 +31,15 @@ extern "C" { #endif -void iksolver_initialize_tree( - struct Depsgraph *depsgraph, struct Scene *scene, struct Object *ob, float ctime); -void iksolver_execute_tree( - struct Depsgraph *depsgraph, struct Scene *scene, struct Object *ob, - struct bPoseChannel *pchan_root, float ctime); +void iksolver_initialize_tree(struct Depsgraph *depsgraph, + struct Scene *scene, + struct Object *ob, + float ctime); +void iksolver_execute_tree(struct Depsgraph *depsgraph, + struct Scene *scene, + struct Object *ob, + struct bPoseChannel *pchan_root, + float ctime); void iksolver_release_tree(struct Scene *scene, struct Object *ob, float ctime); void iksolver_clear_data(struct bPose *pose); @@ -44,4 +47,4 @@ void iksolver_clear_data(struct bPose *pose); } #endif -#endif /* __IKSOLVER_PLUGIN_H__ */ +#endif /* __IKSOLVER_PLUGIN_H__ */ diff --git a/source/blender/ikplugin/intern/itasc_plugin.cpp b/source/blender/ikplugin/intern/itasc_plugin.cpp index dbda188c96c..f20ecc3c11d 100644 --- a/source/blender/ikplugin/intern/itasc_plugin.cpp +++ b/source/blender/ikplugin/intern/itasc_plugin.cpp @@ -28,14 +28,14 @@ // iTaSC headers #ifdef WITH_IK_ITASC -#include "Armature.hpp" -#include "MovingFrame.hpp" -#include "CopyPose.hpp" -#include "WSDLSSolver.hpp" -#include "WDLSSolver.hpp" -#include "Scene.hpp" -#include "Cache.hpp" -#include "Distance.hpp" +# include "Armature.hpp" +# include "MovingFrame.hpp" +# include "CopyPose.hpp" +# include "WSDLSSolver.hpp" +# include "WDLSSolver.hpp" +# include "Scene.hpp" +# include "Cache.hpp" +# include "Distance.hpp" #endif #include "MEM_guardedalloc.h" @@ -64,1776 +64,1872 @@ static bItasc DefIKParam; // in case of animation mode, feedback and timestep is fixed // #define ANIM_TIMESTEP 1.0 -#define ANIM_FEEDBACK 0.8 +#define ANIM_FEEDBACK 0.8 // #define ANIM_QMAX 0.52 - // Structure pointed by bPose.ikdata // It contains everything needed to simulate the armatures // There can be several simulation islands independent to each other struct IK_Data { - struct IK_Scene *first; + struct IK_Scene *first; }; typedef float Vector3[3]; typedef float Vector4[4]; struct IK_Target; -typedef void (*ErrorCallback)(const iTaSC::ConstraintValues *values, unsigned int nvalues, IK_Target *iktarget); +typedef void (*ErrorCallback)(const iTaSC::ConstraintValues *values, + unsigned int nvalues, + IK_Target *iktarget); // one structure for each target in the scene struct IK_Target { - struct Depsgraph *bldepsgraph; - struct Scene *blscene; - iTaSC::MovingFrame* target; - iTaSC::ConstraintSet* constraint; - struct bConstraint* blenderConstraint; - struct bPoseChannel* rootChannel; - Object* owner; //for auto IK - ErrorCallback errorCallback; - std::string targetName; - std::string constraintName; - unsigned short controlType; - short channel; //index in IK channel array of channel on which this target is defined - short ee; //end effector number - bool simulation; //true when simulation mode is used (update feedback) - bool eeBlend; //end effector affected by enforce blending - float eeRest[4][4]; //end effector initial pose relative to armature - - IK_Target() { - bldepsgraph = NULL; - blscene = NULL; - target = NULL; - constraint = NULL; - blenderConstraint = NULL; - rootChannel = NULL; - owner = NULL; - controlType = 0; - channel = 0; - ee = 0; - eeBlend = true; - simulation = true; - targetName.reserve(32); - constraintName.reserve(32); - } - ~IK_Target() { - if (constraint) - delete constraint; - if (target) - delete target; - } + struct Depsgraph *bldepsgraph; + struct Scene *blscene; + iTaSC::MovingFrame *target; + iTaSC::ConstraintSet *constraint; + struct bConstraint *blenderConstraint; + struct bPoseChannel *rootChannel; + Object *owner; //for auto IK + ErrorCallback errorCallback; + std::string targetName; + std::string constraintName; + unsigned short controlType; + short channel; //index in IK channel array of channel on which this target is defined + short ee; //end effector number + bool simulation; //true when simulation mode is used (update feedback) + bool eeBlend; //end effector affected by enforce blending + float eeRest[4][4]; //end effector initial pose relative to armature + + IK_Target() + { + bldepsgraph = NULL; + blscene = NULL; + target = NULL; + constraint = NULL; + blenderConstraint = NULL; + rootChannel = NULL; + owner = NULL; + controlType = 0; + channel = 0; + ee = 0; + eeBlend = true; + simulation = true; + targetName.reserve(32); + constraintName.reserve(32); + } + ~IK_Target() + { + if (constraint) + delete constraint; + if (target) + delete target; + } }; struct IK_Channel { - bPoseChannel* pchan; // channel where we must copy matrix back - KDL::Frame frame; // frame of the bone relative to object base, not armature base - std::string tail; // segment name of the joint from which we get the bone tail - std::string head; // segment name of the joint from which we get the bone head - int parent; // index in this array of the parent channel - short jointType; // type of joint, combination of IK_SegmentFlag - char ndof; // number of joint angles for this channel - char jointValid; // set to 1 when jointValue has been computed - // for joint constraint - Object* owner; // for pose and IK param - double jointValue[4]; // computed joint value - - IK_Channel() { - pchan = NULL; - parent = -1; - jointType = 0; - ndof = 0; - jointValid = 0; - owner = NULL; - jointValue[0] = 0.0; - jointValue[1] = 0.0; - jointValue[2] = 0.0; - jointValue[3] = 0.0; - } + bPoseChannel *pchan; // channel where we must copy matrix back + KDL::Frame frame; // frame of the bone relative to object base, not armature base + std::string tail; // segment name of the joint from which we get the bone tail + std::string head; // segment name of the joint from which we get the bone head + int parent; // index in this array of the parent channel + short jointType; // type of joint, combination of IK_SegmentFlag + char ndof; // number of joint angles for this channel + char jointValid; // set to 1 when jointValue has been computed + // for joint constraint + Object *owner; // for pose and IK param + double jointValue[4]; // computed joint value + + IK_Channel() + { + pchan = NULL; + parent = -1; + jointType = 0; + ndof = 0; + jointValid = 0; + owner = NULL; + jointValue[0] = 0.0; + jointValue[1] = 0.0; + jointValue[2] = 0.0; + jointValue[3] = 0.0; + } }; struct IK_Scene { - struct Depsgraph *bldepsgraph; - struct Scene *blscene; - IK_Scene* next; - int numchan; // number of channel in pchan - int numjoint; // number of joint in jointArray - // array of bone information, one per channel in the tree - IK_Channel* channels; - iTaSC::Armature* armature; - iTaSC::Cache* cache; - iTaSC::Scene* scene; - iTaSC::MovingFrame* base; // armature base object - KDL::Frame baseFrame; // frame of armature base relative to blArmature - KDL::JntArray jointArray; // buffer for storing temporary joint array - iTaSC::Solver* solver; - Object* blArmature; - float blScale; // scale of the Armature object (assume uniform scaling) - float blInvScale; // inverse of Armature object scale - struct bConstraint* polarConstraint; - std::vector<IK_Target*> targets; - - IK_Scene() { - bldepsgraph = NULL; - blscene = NULL; - next = NULL; - channels = NULL; - armature = NULL; - cache = NULL; - scene = NULL; - base = NULL; - solver = NULL; - blScale = blInvScale = 1.0f; - blArmature = NULL; - numchan = 0; - numjoint = 0; - polarConstraint = NULL; - } - - ~IK_Scene() { - // delete scene first - if (scene) - delete scene; - for (std::vector<IK_Target *>::iterator it = targets.begin(); it != targets.end(); ++it) - delete (*it); - targets.clear(); - if (channels) - delete[] channels; - if (solver) - delete solver; - if (armature) - delete armature; - if (base) - delete base; - // delete cache last - if (cache) - delete cache; - } + struct Depsgraph *bldepsgraph; + struct Scene *blscene; + IK_Scene *next; + int numchan; // number of channel in pchan + int numjoint; // number of joint in jointArray + // array of bone information, one per channel in the tree + IK_Channel *channels; + iTaSC::Armature *armature; + iTaSC::Cache *cache; + iTaSC::Scene *scene; + iTaSC::MovingFrame *base; // armature base object + KDL::Frame baseFrame; // frame of armature base relative to blArmature + KDL::JntArray jointArray; // buffer for storing temporary joint array + iTaSC::Solver *solver; + Object *blArmature; + float blScale; // scale of the Armature object (assume uniform scaling) + float blInvScale; // inverse of Armature object scale + struct bConstraint *polarConstraint; + std::vector<IK_Target *> targets; + + IK_Scene() + { + bldepsgraph = NULL; + blscene = NULL; + next = NULL; + channels = NULL; + armature = NULL; + cache = NULL; + scene = NULL; + base = NULL; + solver = NULL; + blScale = blInvScale = 1.0f; + blArmature = NULL; + numchan = 0; + numjoint = 0; + polarConstraint = NULL; + } + + ~IK_Scene() + { + // delete scene first + if (scene) + delete scene; + for (std::vector<IK_Target *>::iterator it = targets.begin(); it != targets.end(); ++it) + delete (*it); + targets.clear(); + if (channels) + delete[] channels; + if (solver) + delete solver; + if (armature) + delete armature; + if (base) + delete base; + // delete cache last + if (cache) + delete cache; + } }; // type of IK joint, can be combined to list the joints corresponding to a bone enum IK_SegmentFlag { - IK_XDOF = 1, - IK_YDOF = 2, - IK_ZDOF = 4, - IK_SWING = 8, - IK_REVOLUTE = 16, - IK_TRANSY = 32, + IK_XDOF = 1, + IK_YDOF = 2, + IK_ZDOF = 4, + IK_SWING = 8, + IK_REVOLUTE = 16, + IK_TRANSY = 32, }; enum IK_SegmentAxis { - IK_X = 0, - IK_Y = 1, - IK_Z = 2, - IK_TRANS_X = 3, - IK_TRANS_Y = 4, - IK_TRANS_Z = 5, + IK_X = 0, + IK_Y = 1, + IK_Z = 2, + IK_TRANS_X = 3, + IK_TRANS_Y = 4, + IK_TRANS_Z = 5, }; static int initialize_chain(Object *ob, bPoseChannel *pchan_tip, bConstraint *con) { - bPoseChannel *curchan, *pchan_root = NULL, *chanlist[256], **oldchan; - PoseTree *tree; - PoseTarget *target; - bKinematicConstraint *data; - int a, t, segcount = 0, size, newsize, *oldparent, parent, rootbone, treecount; - - data = (bKinematicConstraint *)con->data; - - /* exclude tip from chain? */ - if (!(data->flag & CONSTRAINT_IK_TIP)) - pchan_tip = pchan_tip->parent; - - rootbone = data->rootbone; - /* Find the chain's root & count the segments needed */ - for (curchan = pchan_tip; curchan; curchan = curchan->parent) { - pchan_root = curchan; - - if (++segcount > 255) // 255 is weak - break; - - if (segcount == rootbone) { - // reached this end of the chain but if the chain is overlapping with a - // previous one, we must go back up to the root of the other chain - if ((curchan->flag & POSE_CHAIN) && BLI_listbase_is_empty(&curchan->iktree)) { - rootbone++; - continue; - } - break; - } - - if (BLI_listbase_is_empty(&curchan->iktree) == false) - // Oh oh, there is already a chain starting from this channel and our chain is longer... - // Should handle this by moving the previous chain up to the beginning of our chain - // For now we just stop here - break; - } - if (!segcount) return 0; - // we reached a limit and still not the end of a previous chain, quit - if ((pchan_root->flag & POSE_CHAIN) && BLI_listbase_is_empty(&pchan_root->iktree)) return 0; - - // now that we know how many segment we have, set the flag - for (rootbone = segcount, segcount = 0, curchan = pchan_tip; segcount < rootbone; segcount++, curchan = curchan->parent) { - chanlist[segcount] = curchan; - curchan->flag |= POSE_CHAIN; - } - - /* setup the chain data */ - /* create a target */ - target = (PoseTarget *)MEM_callocN(sizeof(PoseTarget), "posetarget"); - target->con = con; - // by contruction there can be only one tree per channel and each channel can be part of at most one tree. - tree = (PoseTree *)pchan_root->iktree.first; - - if (tree == NULL) { - /* make new tree */ - tree = (PoseTree *)MEM_callocN(sizeof(PoseTree), "posetree"); - - tree->iterations = data->iterations; - tree->totchannel = segcount; - tree->stretch = (data->flag & CONSTRAINT_IK_STRETCH); - - tree->pchan = (bPoseChannel **)MEM_callocN(segcount * sizeof(void *), "ik tree pchan"); - tree->parent = (int *)MEM_callocN(segcount * sizeof(int), "ik tree parent"); - for (a = 0; a < segcount; a++) { - tree->pchan[a] = chanlist[segcount - a - 1]; - tree->parent[a] = a - 1; - } - target->tip = segcount - 1; - - /* AND! link the tree to the root */ - BLI_addtail(&pchan_root->iktree, tree); - // new tree - treecount = 1; - } - else { - tree->iterations = MAX2(data->iterations, tree->iterations); - tree->stretch = tree->stretch && !(data->flag & CONSTRAINT_IK_STRETCH); - - /* skip common pose channels and add remaining*/ - size = MIN2(segcount, tree->totchannel); - a = t = 0; - while (a < size && t < tree->totchannel) { - // locate first matching channel - for (; t < tree->totchannel && tree->pchan[t] != chanlist[segcount - a - 1]; t++) ; - if (t >= tree->totchannel) - break; - for (; a < size && t < tree->totchannel && tree->pchan[t] == chanlist[segcount - a - 1]; a++, t++) ; - } - - segcount = segcount - a; - target->tip = tree->totchannel + segcount - 1; - - if (segcount > 0) { - for (parent = a - 1; parent < tree->totchannel; parent++) - if (tree->pchan[parent] == chanlist[segcount - 1]->parent) - break; - - /* shouldn't happen, but could with dependency cycles */ - if (parent == tree->totchannel) - parent = a - 1; - - /* resize array */ - newsize = tree->totchannel + segcount; - oldchan = tree->pchan; - oldparent = tree->parent; - - tree->pchan = (bPoseChannel **)MEM_callocN(newsize * sizeof(void *), "ik tree pchan"); - tree->parent = (int *)MEM_callocN(newsize * sizeof(int), "ik tree parent"); - memcpy(tree->pchan, oldchan, sizeof(void *) * tree->totchannel); - memcpy(tree->parent, oldparent, sizeof(int) * tree->totchannel); - MEM_freeN(oldchan); - MEM_freeN(oldparent); - - /* add new pose channels at the end, in reverse order */ - for (a = 0; a < segcount; a++) { - tree->pchan[tree->totchannel + a] = chanlist[segcount - a - 1]; - tree->parent[tree->totchannel + a] = tree->totchannel + a - 1; - } - tree->parent[tree->totchannel] = parent; - - tree->totchannel = newsize; - } - // reusing tree - treecount = 0; - } - - /* add target to the tree */ - BLI_addtail(&tree->targets, target); - /* mark root channel having an IK tree */ - pchan_root->flag |= POSE_IKTREE; - return treecount; + bPoseChannel *curchan, *pchan_root = NULL, *chanlist[256], **oldchan; + PoseTree *tree; + PoseTarget *target; + bKinematicConstraint *data; + int a, t, segcount = 0, size, newsize, *oldparent, parent, rootbone, treecount; + + data = (bKinematicConstraint *)con->data; + + /* exclude tip from chain? */ + if (!(data->flag & CONSTRAINT_IK_TIP)) + pchan_tip = pchan_tip->parent; + + rootbone = data->rootbone; + /* Find the chain's root & count the segments needed */ + for (curchan = pchan_tip; curchan; curchan = curchan->parent) { + pchan_root = curchan; + + if (++segcount > 255) // 255 is weak + break; + + if (segcount == rootbone) { + // reached this end of the chain but if the chain is overlapping with a + // previous one, we must go back up to the root of the other chain + if ((curchan->flag & POSE_CHAIN) && BLI_listbase_is_empty(&curchan->iktree)) { + rootbone++; + continue; + } + break; + } + + if (BLI_listbase_is_empty(&curchan->iktree) == false) + // Oh oh, there is already a chain starting from this channel and our chain is longer... + // Should handle this by moving the previous chain up to the beginning of our chain + // For now we just stop here + break; + } + if (!segcount) + return 0; + // we reached a limit and still not the end of a previous chain, quit + if ((pchan_root->flag & POSE_CHAIN) && BLI_listbase_is_empty(&pchan_root->iktree)) + return 0; + + // now that we know how many segment we have, set the flag + for (rootbone = segcount, segcount = 0, curchan = pchan_tip; segcount < rootbone; + segcount++, curchan = curchan->parent) { + chanlist[segcount] = curchan; + curchan->flag |= POSE_CHAIN; + } + + /* setup the chain data */ + /* create a target */ + target = (PoseTarget *)MEM_callocN(sizeof(PoseTarget), "posetarget"); + target->con = con; + // by contruction there can be only one tree per channel and each channel can be part of at most one tree. + tree = (PoseTree *)pchan_root->iktree.first; + + if (tree == NULL) { + /* make new tree */ + tree = (PoseTree *)MEM_callocN(sizeof(PoseTree), "posetree"); + + tree->iterations = data->iterations; + tree->totchannel = segcount; + tree->stretch = (data->flag & CONSTRAINT_IK_STRETCH); + + tree->pchan = (bPoseChannel **)MEM_callocN(segcount * sizeof(void *), "ik tree pchan"); + tree->parent = (int *)MEM_callocN(segcount * sizeof(int), "ik tree parent"); + for (a = 0; a < segcount; a++) { + tree->pchan[a] = chanlist[segcount - a - 1]; + tree->parent[a] = a - 1; + } + target->tip = segcount - 1; + + /* AND! link the tree to the root */ + BLI_addtail(&pchan_root->iktree, tree); + // new tree + treecount = 1; + } + else { + tree->iterations = MAX2(data->iterations, tree->iterations); + tree->stretch = tree->stretch && !(data->flag & CONSTRAINT_IK_STRETCH); + + /* skip common pose channels and add remaining*/ + size = MIN2(segcount, tree->totchannel); + a = t = 0; + while (a < size && t < tree->totchannel) { + // locate first matching channel + for (; t < tree->totchannel && tree->pchan[t] != chanlist[segcount - a - 1]; t++) + ; + if (t >= tree->totchannel) + break; + for (; a < size && t < tree->totchannel && tree->pchan[t] == chanlist[segcount - a - 1]; + a++, t++) + ; + } + + segcount = segcount - a; + target->tip = tree->totchannel + segcount - 1; + + if (segcount > 0) { + for (parent = a - 1; parent < tree->totchannel; parent++) + if (tree->pchan[parent] == chanlist[segcount - 1]->parent) + break; + + /* shouldn't happen, but could with dependency cycles */ + if (parent == tree->totchannel) + parent = a - 1; + + /* resize array */ + newsize = tree->totchannel + segcount; + oldchan = tree->pchan; + oldparent = tree->parent; + + tree->pchan = (bPoseChannel **)MEM_callocN(newsize * sizeof(void *), "ik tree pchan"); + tree->parent = (int *)MEM_callocN(newsize * sizeof(int), "ik tree parent"); + memcpy(tree->pchan, oldchan, sizeof(void *) * tree->totchannel); + memcpy(tree->parent, oldparent, sizeof(int) * tree->totchannel); + MEM_freeN(oldchan); + MEM_freeN(oldparent); + + /* add new pose channels at the end, in reverse order */ + for (a = 0; a < segcount; a++) { + tree->pchan[tree->totchannel + a] = chanlist[segcount - a - 1]; + tree->parent[tree->totchannel + a] = tree->totchannel + a - 1; + } + tree->parent[tree->totchannel] = parent; + + tree->totchannel = newsize; + } + // reusing tree + treecount = 0; + } + + /* add target to the tree */ + BLI_addtail(&tree->targets, target); + /* mark root channel having an IK tree */ + pchan_root->flag |= POSE_IKTREE; + return treecount; } static bool is_cartesian_constraint(bConstraint *con) { - //bKinematicConstraint* data=(bKinematicConstraint *)con->data; + //bKinematicConstraint* data=(bKinematicConstraint *)con->data; - return true; + return true; } static bool constraint_valid(bConstraint *con) { - bKinematicConstraint *data = (bKinematicConstraint *)con->data; - - if (data->flag & CONSTRAINT_IK_AUTO) - return true; - if (con->flag & (CONSTRAINT_DISABLE | CONSTRAINT_OFF)) - return false; - if (is_cartesian_constraint(con)) { - /* cartesian space constraint */ - if (data->tar == NULL) - return false; - if (data->tar->type == OB_ARMATURE && data->subtarget[0] == 0) - return false; - } - return true; + bKinematicConstraint *data = (bKinematicConstraint *)con->data; + + if (data->flag & CONSTRAINT_IK_AUTO) + return true; + if (con->flag & (CONSTRAINT_DISABLE | CONSTRAINT_OFF)) + return false; + if (is_cartesian_constraint(con)) { + /* cartesian space constraint */ + if (data->tar == NULL) + return false; + if (data->tar->type == OB_ARMATURE && data->subtarget[0] == 0) + return false; + } + return true; } static int initialize_scene(Object *ob, bPoseChannel *pchan_tip) { - bConstraint *con; - int treecount; - - /* find all IK constraints and validate them */ - treecount = 0; - for (con = (bConstraint *)pchan_tip->constraints.first; con; con = (bConstraint *)con->next) { - if (con->type == CONSTRAINT_TYPE_KINEMATIC) { - if (constraint_valid(con)) - treecount += initialize_chain(ob, pchan_tip, con); - } - } - return treecount; + bConstraint *con; + int treecount; + + /* find all IK constraints and validate them */ + treecount = 0; + for (con = (bConstraint *)pchan_tip->constraints.first; con; con = (bConstraint *)con->next) { + if (con->type == CONSTRAINT_TYPE_KINEMATIC) { + if (constraint_valid(con)) + treecount += initialize_chain(ob, pchan_tip, con); + } + } + return treecount; } static IK_Data *get_ikdata(bPose *pose) { - if (pose->ikdata) - return (IK_Data *)pose->ikdata; - pose->ikdata = MEM_callocN(sizeof(IK_Data), "iTaSC ikdata"); - // here init ikdata if needed - // now that we have scene, make sure the default param are initialized - if (!DefIKParam.iksolver) - BKE_pose_itasc_init(&DefIKParam); - - return (IK_Data *)pose->ikdata; + if (pose->ikdata) + return (IK_Data *)pose->ikdata; + pose->ikdata = MEM_callocN(sizeof(IK_Data), "iTaSC ikdata"); + // here init ikdata if needed + // now that we have scene, make sure the default param are initialized + if (!DefIKParam.iksolver) + BKE_pose_itasc_init(&DefIKParam); + + return (IK_Data *)pose->ikdata; } -static double EulerAngleFromMatrix(const KDL::Rotation& R, int axis) +static double EulerAngleFromMatrix(const KDL::Rotation &R, int axis) { - double t = KDL::sqrt(R(0, 0) * R(0, 0) + R(0, 1) * R(0, 1)); - - if (t > 16.0 * KDL::epsilon) { - if (axis == 0) return -KDL::atan2(R(1, 2), R(2, 2)); - else if (axis == 1) return KDL::atan2(-R(0, 2), t); - else return -KDL::atan2(R(0, 1), R(0, 0)); - } - else { - if (axis == 0) return -KDL::atan2(-R(2, 1), R(1, 1)); - else if (axis == 1) return KDL::atan2(-R(0, 2), t); - else return 0.0f; - } + double t = KDL::sqrt(R(0, 0) * R(0, 0) + R(0, 1) * R(0, 1)); + + if (t > 16.0 * KDL::epsilon) { + if (axis == 0) + return -KDL::atan2(R(1, 2), R(2, 2)); + else if (axis == 1) + return KDL::atan2(-R(0, 2), t); + else + return -KDL::atan2(R(0, 1), R(0, 0)); + } + else { + if (axis == 0) + return -KDL::atan2(-R(2, 1), R(1, 1)); + else if (axis == 1) + return KDL::atan2(-R(0, 2), t); + else + return 0.0f; + } } -static double ComputeTwist(const KDL::Rotation& R) +static double ComputeTwist(const KDL::Rotation &R) { - // qy and qw are the y and w components of the quaternion from R - double qy = R(0, 2) - R(2, 0); - double qw = R(0, 0) + R(1, 1) + R(2, 2) + 1; + // qy and qw are the y and w components of the quaternion from R + double qy = R(0, 2) - R(2, 0); + double qw = R(0, 0) + R(1, 1) + R(2, 2) + 1; - double tau = 2 * KDL::atan2(qy, qw); + double tau = 2 * KDL::atan2(qy, qw); - return tau; + return tau; } -static void RemoveEulerAngleFromMatrix(KDL::Rotation& R, double angle, int axis) +static void RemoveEulerAngleFromMatrix(KDL::Rotation &R, double angle, int axis) { - // compute twist parameter - KDL::Rotation T; - switch (axis) { - case 0: - T = KDL::Rotation::RotX(-angle); - break; - case 1: - T = KDL::Rotation::RotY(-angle); - break; - case 2: - T = KDL::Rotation::RotZ(-angle); - break; - default: - return; - } - // remove angle - R = R * T; + // compute twist parameter + KDL::Rotation T; + switch (axis) { + case 0: + T = KDL::Rotation::RotX(-angle); + break; + case 1: + T = KDL::Rotation::RotY(-angle); + break; + case 2: + T = KDL::Rotation::RotZ(-angle); + break; + default: + return; + } + // remove angle + R = R * T; } #if 0 static void GetEulerXZY(const KDL::Rotation& R, double& X, double& Z, double& Y) { - if (fabs(R(0, 1)) > 1.0 - KDL::epsilon) { - X = -KDL::sign(R(0, 1)) * KDL::atan2(R(1, 2), R(1, 0)); - Z = -KDL::sign(R(0, 1)) * KDL::PI / 2; - Y = 0.0; - } - else { - X = KDL::atan2(R(2, 1), R(1, 1)); - Z = KDL::atan2(-R(0, 1), KDL::sqrt(KDL::sqr(R(0, 0)) + KDL::sqr(R(0, 2)))); - Y = KDL::atan2(R(0, 2), R(0, 0)); - } + if (fabs(R(0, 1)) > 1.0 - KDL::epsilon) { + X = -KDL::sign(R(0, 1)) * KDL::atan2(R(1, 2), R(1, 0)); + Z = -KDL::sign(R(0, 1)) * KDL::PI / 2; + Y = 0.0; + } + else { + X = KDL::atan2(R(2, 1), R(1, 1)); + Z = KDL::atan2(-R(0, 1), KDL::sqrt(KDL::sqr(R(0, 0)) + KDL::sqr(R(0, 2)))); + Y = KDL::atan2(R(0, 2), R(0, 0)); + } } static void GetEulerXYZ(const KDL::Rotation& R, double& X, double& Y, double& Z) { - if (fabs(R(0, 2)) > 1.0 - KDL::epsilon) { - X = KDL::sign(R(0, 2)) * KDL::atan2(-R(1, 0), R(1, 1)); - Y = KDL::sign(R(0, 2)) * KDL::PI / 2; - Z = 0.0; - } - else { - X = KDL::atan2(-R(1, 2), R(2, 2)); - Y = KDL::atan2(R(0, 2), KDL::sqrt(KDL::sqr(R(0, 0)) + KDL::sqr(R(0, 1)))); - Z = KDL::atan2(-R(0, 1), R(0, 0)); - } + if (fabs(R(0, 2)) > 1.0 - KDL::epsilon) { + X = KDL::sign(R(0, 2)) * KDL::atan2(-R(1, 0), R(1, 1)); + Y = KDL::sign(R(0, 2)) * KDL::PI / 2; + Z = 0.0; + } + else { + X = KDL::atan2(-R(1, 2), R(2, 2)); + Y = KDL::atan2(R(0, 2), KDL::sqrt(KDL::sqr(R(0, 0)) + KDL::sqr(R(0, 1)))); + Z = KDL::atan2(-R(0, 1), R(0, 0)); + } } #endif -static void GetJointRotation(KDL::Rotation& boneRot, int type, double *rot) +static void GetJointRotation(KDL::Rotation &boneRot, int type, double *rot) { - switch (type & ~IK_TRANSY) { - default: - // fixed bone, no joint - break; - case IK_XDOF: - // RX only, get the X rotation - rot[0] = EulerAngleFromMatrix(boneRot, 0); - break; - case IK_YDOF: - // RY only, get the Y rotation - rot[0] = ComputeTwist(boneRot); - break; - case IK_ZDOF: - // RZ only, get the Z rotation - rot[0] = EulerAngleFromMatrix(boneRot, 2); - break; - case IK_XDOF | IK_YDOF: - rot[1] = ComputeTwist(boneRot); - RemoveEulerAngleFromMatrix(boneRot, rot[1], 1); - rot[0] = EulerAngleFromMatrix(boneRot, 0); - break; - case IK_SWING: - // RX+RZ - boneRot.GetXZRot().GetValue(rot); - break; - case IK_YDOF | IK_ZDOF: - // RZ+RY - rot[1] = ComputeTwist(boneRot); - RemoveEulerAngleFromMatrix(boneRot, rot[1], 1); - rot[0] = EulerAngleFromMatrix(boneRot, 2); - break; - case IK_SWING | IK_YDOF: - rot[2] = ComputeTwist(boneRot); - RemoveEulerAngleFromMatrix(boneRot, rot[2], 1); - boneRot.GetXZRot().GetValue(rot); - break; - case IK_REVOLUTE: - boneRot.GetRot().GetValue(rot); - break; - } + switch (type & ~IK_TRANSY) { + default: + // fixed bone, no joint + break; + case IK_XDOF: + // RX only, get the X rotation + rot[0] = EulerAngleFromMatrix(boneRot, 0); + break; + case IK_YDOF: + // RY only, get the Y rotation + rot[0] = ComputeTwist(boneRot); + break; + case IK_ZDOF: + // RZ only, get the Z rotation + rot[0] = EulerAngleFromMatrix(boneRot, 2); + break; + case IK_XDOF | IK_YDOF: + rot[1] = ComputeTwist(boneRot); + RemoveEulerAngleFromMatrix(boneRot, rot[1], 1); + rot[0] = EulerAngleFromMatrix(boneRot, 0); + break; + case IK_SWING: + // RX+RZ + boneRot.GetXZRot().GetValue(rot); + break; + case IK_YDOF | IK_ZDOF: + // RZ+RY + rot[1] = ComputeTwist(boneRot); + RemoveEulerAngleFromMatrix(boneRot, rot[1], 1); + rot[0] = EulerAngleFromMatrix(boneRot, 2); + break; + case IK_SWING | IK_YDOF: + rot[2] = ComputeTwist(boneRot); + RemoveEulerAngleFromMatrix(boneRot, rot[2], 1); + boneRot.GetXZRot().GetValue(rot); + break; + case IK_REVOLUTE: + boneRot.GetRot().GetValue(rot); + break; + } } -static bool target_callback(const iTaSC::Timestamp& timestamp, const iTaSC::Frame& current, iTaSC::Frame& next, void *param) +static bool target_callback(const iTaSC::Timestamp ×tamp, + const iTaSC::Frame ¤t, + iTaSC::Frame &next, + void *param) { - IK_Target *target = (IK_Target *)param; - // compute next target position - // get target matrix from constraint. - bConstraint *constraint = (bConstraint *)target->blenderConstraint; - float tarmat[4][4]; - - BKE_constraint_target_matrix_get(target->bldepsgraph, target->blscene, constraint, 0, CONSTRAINT_OBTYPE_OBJECT, target->owner, tarmat, 1.0); - - // rootmat contains the target pose in world coordinate - // if enforce is != 1.0, blend the target position with the end effector position - // if the armature was in rest position. This information is available in eeRest - if (constraint->enforce != 1.0f && target->eeBlend) { - // eeRest is relative to the reference frame of the IK root - // get this frame in world reference - float restmat[4][4]; - bPoseChannel *pchan = target->rootChannel; - if (pchan->parent) { - pchan = pchan->parent; - float chanmat[4][4]; - copy_m4_m4(chanmat, pchan->pose_mat); - copy_v3_v3(chanmat[3], pchan->pose_tail); - mul_m4_series(restmat, target->owner->obmat, chanmat, target->eeRest); - } - else { - mul_m4_m4m4(restmat, target->owner->obmat, target->eeRest); - } - // blend the target - blend_m4_m4m4(tarmat, restmat, tarmat, constraint->enforce); - } - next.setValue(&tarmat[0][0]); - return true; + IK_Target *target = (IK_Target *)param; + // compute next target position + // get target matrix from constraint. + bConstraint *constraint = (bConstraint *)target->blenderConstraint; + float tarmat[4][4]; + + BKE_constraint_target_matrix_get(target->bldepsgraph, + target->blscene, + constraint, + 0, + CONSTRAINT_OBTYPE_OBJECT, + target->owner, + tarmat, + 1.0); + + // rootmat contains the target pose in world coordinate + // if enforce is != 1.0, blend the target position with the end effector position + // if the armature was in rest position. This information is available in eeRest + if (constraint->enforce != 1.0f && target->eeBlend) { + // eeRest is relative to the reference frame of the IK root + // get this frame in world reference + float restmat[4][4]; + bPoseChannel *pchan = target->rootChannel; + if (pchan->parent) { + pchan = pchan->parent; + float chanmat[4][4]; + copy_m4_m4(chanmat, pchan->pose_mat); + copy_v3_v3(chanmat[3], pchan->pose_tail); + mul_m4_series(restmat, target->owner->obmat, chanmat, target->eeRest); + } + else { + mul_m4_m4m4(restmat, target->owner->obmat, target->eeRest); + } + // blend the target + blend_m4_m4m4(tarmat, restmat, tarmat, constraint->enforce); + } + next.setValue(&tarmat[0][0]); + return true; } -static bool base_callback(const iTaSC::Timestamp& timestamp, const iTaSC::Frame& current, iTaSC::Frame& next, void *param) +static bool base_callback(const iTaSC::Timestamp ×tamp, + const iTaSC::Frame ¤t, + iTaSC::Frame &next, + void *param) { - IK_Scene *ikscene = (IK_Scene *)param; - // compute next armature base pose - // algorithm: - // ikscene->pchan[0] is the root channel of the tree - // if it has a parent, get the pose matrix from it and replace [3] by parent pchan->tail - // then multiply by the armature matrix to get ikscene->armature base position - bPoseChannel *pchan = ikscene->channels[0].pchan; - float rootmat[4][4]; - if (pchan->parent) { - pchan = pchan->parent; - float chanmat[4][4]; - copy_m4_m4(chanmat, pchan->pose_mat); - copy_v3_v3(chanmat[3], pchan->pose_tail); - // save the base as a frame too so that we can compute deformation after simulation - ikscene->baseFrame.setValue(&chanmat[0][0]); - // iTaSC armature is scaled to object scale, scale the base frame too - ikscene->baseFrame.p *= ikscene->blScale; - mul_m4_m4m4(rootmat, ikscene->blArmature->obmat, chanmat); - } - else { - copy_m4_m4(rootmat, ikscene->blArmature->obmat); - ikscene->baseFrame = iTaSC::F_identity; - } - next.setValue(&rootmat[0][0]); - // if there is a polar target (only during solving otherwise we don't have end efffector) - if (ikscene->polarConstraint && timestamp.update) { - // compute additional rotation of base frame so that armature follows the polar target - float imat[4][4]; // IK tree base inverse matrix - float polemat[4][4]; // polar target in IK tree base frame - float goalmat[4][4]; // target in IK tree base frame - float mat[4][4]; // temp matrix - bKinematicConstraint *poledata = (bKinematicConstraint *)ikscene->polarConstraint->data; - - invert_m4_m4(imat, rootmat); - // polar constraint imply only one target - IK_Target *iktarget = ikscene->targets[0]; - // root channel from which we take the bone initial orientation - IK_Channel &rootchan = ikscene->channels[0]; - - // get polar target matrix in world space - BKE_constraint_target_matrix_get(ikscene->bldepsgraph, ikscene->blscene, ikscene->polarConstraint, 1, CONSTRAINT_OBTYPE_OBJECT, ikscene->blArmature, mat, 1.0); - // convert to armature space - mul_m4_m4m4(polemat, imat, mat); - // get the target in world space (was computed before as target object are defined before base object) - iktarget->target->getPose().getValue(mat[0]); - // convert to armature space - mul_m4_m4m4(goalmat, imat, mat); - // take position of target, polar target, end effector, in armature space - KDL::Vector goalpos(goalmat[3]); - KDL::Vector polepos(polemat[3]); - KDL::Vector endpos = ikscene->armature->getPose(iktarget->ee).p; - // get root bone orientation - KDL::Frame rootframe; - ikscene->armature->getRelativeFrame(rootframe, rootchan.tail); - KDL::Vector rootx = rootframe.M.UnitX(); - KDL::Vector rootz = rootframe.M.UnitZ(); - // and compute root bone head - double q_rest[3], q[3], length; - const KDL::Joint *joint; - const KDL::Frame *tip; - ikscene->armature->getSegment(rootchan.tail, 3, joint, q_rest[0], q[0], tip); - length = (joint->getType() == KDL::Joint::TransY) ? q[0] : tip->p(1); - KDL::Vector rootpos = rootframe.p - length *rootframe.M.UnitY(); - - // compute main directions - KDL::Vector dir = KDL::Normalize(endpos - rootpos); - KDL::Vector poledir = KDL::Normalize(goalpos - rootpos); - // compute up directions - KDL::Vector poleup = KDL::Normalize(polepos - rootpos); - KDL::Vector up = rootx * KDL::cos(poledata->poleangle) + rootz *KDL::sin(poledata->poleangle); - // from which we build rotation matrix - KDL::Rotation endrot, polerot; - // for the armature, using the root bone orientation - KDL::Vector x = KDL::Normalize(dir * up); - endrot.UnitX(x); - endrot.UnitY(KDL::Normalize(x * dir)); - endrot.UnitZ(-dir); - // for the polar target - x = KDL::Normalize(poledir * poleup); - polerot.UnitX(x); - polerot.UnitY(KDL::Normalize(x * poledir)); - polerot.UnitZ(-poledir); - // the difference between the two is the rotation we want to apply - KDL::Rotation result(polerot * endrot.Inverse()); - // apply on base frame as this is an artificial additional rotation - next.M = next.M * result; - ikscene->baseFrame.M = ikscene->baseFrame.M * result; - } - return true; + IK_Scene *ikscene = (IK_Scene *)param; + // compute next armature base pose + // algorithm: + // ikscene->pchan[0] is the root channel of the tree + // if it has a parent, get the pose matrix from it and replace [3] by parent pchan->tail + // then multiply by the armature matrix to get ikscene->armature base position + bPoseChannel *pchan = ikscene->channels[0].pchan; + float rootmat[4][4]; + if (pchan->parent) { + pchan = pchan->parent; + float chanmat[4][4]; + copy_m4_m4(chanmat, pchan->pose_mat); + copy_v3_v3(chanmat[3], pchan->pose_tail); + // save the base as a frame too so that we can compute deformation after simulation + ikscene->baseFrame.setValue(&chanmat[0][0]); + // iTaSC armature is scaled to object scale, scale the base frame too + ikscene->baseFrame.p *= ikscene->blScale; + mul_m4_m4m4(rootmat, ikscene->blArmature->obmat, chanmat); + } + else { + copy_m4_m4(rootmat, ikscene->blArmature->obmat); + ikscene->baseFrame = iTaSC::F_identity; + } + next.setValue(&rootmat[0][0]); + // if there is a polar target (only during solving otherwise we don't have end efffector) + if (ikscene->polarConstraint && timestamp.update) { + // compute additional rotation of base frame so that armature follows the polar target + float imat[4][4]; // IK tree base inverse matrix + float polemat[4][4]; // polar target in IK tree base frame + float goalmat[4][4]; // target in IK tree base frame + float mat[4][4]; // temp matrix + bKinematicConstraint *poledata = (bKinematicConstraint *)ikscene->polarConstraint->data; + + invert_m4_m4(imat, rootmat); + // polar constraint imply only one target + IK_Target *iktarget = ikscene->targets[0]; + // root channel from which we take the bone initial orientation + IK_Channel &rootchan = ikscene->channels[0]; + + // get polar target matrix in world space + BKE_constraint_target_matrix_get(ikscene->bldepsgraph, + ikscene->blscene, + ikscene->polarConstraint, + 1, + CONSTRAINT_OBTYPE_OBJECT, + ikscene->blArmature, + mat, + 1.0); + // convert to armature space + mul_m4_m4m4(polemat, imat, mat); + // get the target in world space (was computed before as target object are defined before base object) + iktarget->target->getPose().getValue(mat[0]); + // convert to armature space + mul_m4_m4m4(goalmat, imat, mat); + // take position of target, polar target, end effector, in armature space + KDL::Vector goalpos(goalmat[3]); + KDL::Vector polepos(polemat[3]); + KDL::Vector endpos = ikscene->armature->getPose(iktarget->ee).p; + // get root bone orientation + KDL::Frame rootframe; + ikscene->armature->getRelativeFrame(rootframe, rootchan.tail); + KDL::Vector rootx = rootframe.M.UnitX(); + KDL::Vector rootz = rootframe.M.UnitZ(); + // and compute root bone head + double q_rest[3], q[3], length; + const KDL::Joint *joint; + const KDL::Frame *tip; + ikscene->armature->getSegment(rootchan.tail, 3, joint, q_rest[0], q[0], tip); + length = (joint->getType() == KDL::Joint::TransY) ? q[0] : tip->p(1); + KDL::Vector rootpos = rootframe.p - length * rootframe.M.UnitY(); + + // compute main directions + KDL::Vector dir = KDL::Normalize(endpos - rootpos); + KDL::Vector poledir = KDL::Normalize(goalpos - rootpos); + // compute up directions + KDL::Vector poleup = KDL::Normalize(polepos - rootpos); + KDL::Vector up = rootx * KDL::cos(poledata->poleangle) + rootz * KDL::sin(poledata->poleangle); + // from which we build rotation matrix + KDL::Rotation endrot, polerot; + // for the armature, using the root bone orientation + KDL::Vector x = KDL::Normalize(dir * up); + endrot.UnitX(x); + endrot.UnitY(KDL::Normalize(x * dir)); + endrot.UnitZ(-dir); + // for the polar target + x = KDL::Normalize(poledir * poleup); + polerot.UnitX(x); + polerot.UnitY(KDL::Normalize(x * poledir)); + polerot.UnitZ(-poledir); + // the difference between the two is the rotation we want to apply + KDL::Rotation result(polerot * endrot.Inverse()); + // apply on base frame as this is an artificial additional rotation + next.M = next.M * result; + ikscene->baseFrame.M = ikscene->baseFrame.M * result; + } + return true; } -static bool copypose_callback(const iTaSC::Timestamp& timestamp, iTaSC::ConstraintValues *const _values, unsigned int _nvalues, void *_param) +static bool copypose_callback(const iTaSC::Timestamp ×tamp, + iTaSC::ConstraintValues *const _values, + unsigned int _nvalues, + void *_param) { - IK_Target *iktarget = (IK_Target *)_param; - bKinematicConstraint *condata = (bKinematicConstraint *)iktarget->blenderConstraint->data; - iTaSC::ConstraintValues *values = _values; - bItasc *ikparam = (bItasc *) iktarget->owner->pose->ikparam; - - // we need default parameters - if (!ikparam) - ikparam = &DefIKParam; - - if (iktarget->blenderConstraint->flag & CONSTRAINT_OFF) { - if (iktarget->controlType & iTaSC::CopyPose::CTL_POSITION) { - values->alpha = 0.0; - values->action = iTaSC::ACT_ALPHA; - values++; - } - if (iktarget->controlType & iTaSC::CopyPose::CTL_ROTATION) { - values->alpha = 0.0; - values->action = iTaSC::ACT_ALPHA; - values++; - } - } - else { - if (iktarget->controlType & iTaSC::CopyPose::CTL_POSITION) { - // update error - values->alpha = condata->weight; - values->action = iTaSC::ACT_ALPHA | iTaSC::ACT_FEEDBACK; - values->feedback = (iktarget->simulation) ? ikparam->feedback : ANIM_FEEDBACK; - values++; - } - if (iktarget->controlType & iTaSC::CopyPose::CTL_ROTATION) { - // update error - values->alpha = condata->orientweight; - values->action = iTaSC::ACT_ALPHA | iTaSC::ACT_FEEDBACK; - values->feedback = (iktarget->simulation) ? ikparam->feedback : ANIM_FEEDBACK; - values++; - } - } - return true; + IK_Target *iktarget = (IK_Target *)_param; + bKinematicConstraint *condata = (bKinematicConstraint *)iktarget->blenderConstraint->data; + iTaSC::ConstraintValues *values = _values; + bItasc *ikparam = (bItasc *)iktarget->owner->pose->ikparam; + + // we need default parameters + if (!ikparam) + ikparam = &DefIKParam; + + if (iktarget->blenderConstraint->flag & CONSTRAINT_OFF) { + if (iktarget->controlType & iTaSC::CopyPose::CTL_POSITION) { + values->alpha = 0.0; + values->action = iTaSC::ACT_ALPHA; + values++; + } + if (iktarget->controlType & iTaSC::CopyPose::CTL_ROTATION) { + values->alpha = 0.0; + values->action = iTaSC::ACT_ALPHA; + values++; + } + } + else { + if (iktarget->controlType & iTaSC::CopyPose::CTL_POSITION) { + // update error + values->alpha = condata->weight; + values->action = iTaSC::ACT_ALPHA | iTaSC::ACT_FEEDBACK; + values->feedback = (iktarget->simulation) ? ikparam->feedback : ANIM_FEEDBACK; + values++; + } + if (iktarget->controlType & iTaSC::CopyPose::CTL_ROTATION) { + // update error + values->alpha = condata->orientweight; + values->action = iTaSC::ACT_ALPHA | iTaSC::ACT_FEEDBACK; + values->feedback = (iktarget->simulation) ? ikparam->feedback : ANIM_FEEDBACK; + values++; + } + } + return true; } -static void copypose_error(const iTaSC::ConstraintValues *values, unsigned int nvalues, IK_Target *iktarget) +static void copypose_error(const iTaSC::ConstraintValues *values, + unsigned int nvalues, + IK_Target *iktarget) { - iTaSC::ConstraintSingleValue *value; - double error; - int i; - - if (iktarget->controlType & iTaSC::CopyPose::CTL_POSITION) { - // update error - for (i = 0, error = 0.0, value = values->values; i < values->number; ++i, ++value) - error += KDL::sqr(value->y - value->yd); - iktarget->blenderConstraint->lin_error = (float)KDL::sqrt(error); - values++; - } - if (iktarget->controlType & iTaSC::CopyPose::CTL_ROTATION) { - // update error - for (i = 0, error = 0.0, value = values->values; i < values->number; ++i, ++value) - error += KDL::sqr(value->y - value->yd); - iktarget->blenderConstraint->rot_error = (float)KDL::sqrt(error); - values++; - } + iTaSC::ConstraintSingleValue *value; + double error; + int i; + + if (iktarget->controlType & iTaSC::CopyPose::CTL_POSITION) { + // update error + for (i = 0, error = 0.0, value = values->values; i < values->number; ++i, ++value) + error += KDL::sqr(value->y - value->yd); + iktarget->blenderConstraint->lin_error = (float)KDL::sqrt(error); + values++; + } + if (iktarget->controlType & iTaSC::CopyPose::CTL_ROTATION) { + // update error + for (i = 0, error = 0.0, value = values->values; i < values->number; ++i, ++value) + error += KDL::sqr(value->y - value->yd); + iktarget->blenderConstraint->rot_error = (float)KDL::sqrt(error); + values++; + } } -static bool distance_callback(const iTaSC::Timestamp& timestamp, iTaSC::ConstraintValues *const _values, unsigned int _nvalues, void *_param) +static bool distance_callback(const iTaSC::Timestamp ×tamp, + iTaSC::ConstraintValues *const _values, + unsigned int _nvalues, + void *_param) { - IK_Target *iktarget = (IK_Target *)_param; - bKinematicConstraint *condata = (bKinematicConstraint *)iktarget->blenderConstraint->data; - iTaSC::ConstraintValues *values = _values; - bItasc *ikparam = (bItasc *) iktarget->owner->pose->ikparam; - // we need default parameters - if (!ikparam) - ikparam = &DefIKParam; - - // update weight according to mode - if (iktarget->blenderConstraint->flag & CONSTRAINT_OFF) { - values->alpha = 0.0; - } - else { - switch (condata->mode) { - case LIMITDIST_INSIDE: - values->alpha = (values->values[0].y > condata->dist) ? condata->weight : 0.0; - break; - case LIMITDIST_OUTSIDE: - values->alpha = (values->values[0].y < condata->dist) ? condata->weight : 0.0; - break; - default: - values->alpha = condata->weight; - break; - } - if (!timestamp.substep) { - // only update value on first timestep - switch (condata->mode) { - case LIMITDIST_INSIDE: - values->values[0].yd = condata->dist * 0.95; - break; - case LIMITDIST_OUTSIDE: - values->values[0].yd = condata->dist * 1.05; - break; - default: - values->values[0].yd = condata->dist; - break; - } - values->values[0].action = iTaSC::ACT_VALUE | iTaSC::ACT_FEEDBACK; - values->feedback = (iktarget->simulation) ? ikparam->feedback : ANIM_FEEDBACK; - } - } - values->action |= iTaSC::ACT_ALPHA; - return true; + IK_Target *iktarget = (IK_Target *)_param; + bKinematicConstraint *condata = (bKinematicConstraint *)iktarget->blenderConstraint->data; + iTaSC::ConstraintValues *values = _values; + bItasc *ikparam = (bItasc *)iktarget->owner->pose->ikparam; + // we need default parameters + if (!ikparam) + ikparam = &DefIKParam; + + // update weight according to mode + if (iktarget->blenderConstraint->flag & CONSTRAINT_OFF) { + values->alpha = 0.0; + } + else { + switch (condata->mode) { + case LIMITDIST_INSIDE: + values->alpha = (values->values[0].y > condata->dist) ? condata->weight : 0.0; + break; + case LIMITDIST_OUTSIDE: + values->alpha = (values->values[0].y < condata->dist) ? condata->weight : 0.0; + break; + default: + values->alpha = condata->weight; + break; + } + if (!timestamp.substep) { + // only update value on first timestep + switch (condata->mode) { + case LIMITDIST_INSIDE: + values->values[0].yd = condata->dist * 0.95; + break; + case LIMITDIST_OUTSIDE: + values->values[0].yd = condata->dist * 1.05; + break; + default: + values->values[0].yd = condata->dist; + break; + } + values->values[0].action = iTaSC::ACT_VALUE | iTaSC::ACT_FEEDBACK; + values->feedback = (iktarget->simulation) ? ikparam->feedback : ANIM_FEEDBACK; + } + } + values->action |= iTaSC::ACT_ALPHA; + return true; } -static void distance_error(const iTaSC::ConstraintValues *values, unsigned int _nvalues, IK_Target *iktarget) +static void distance_error(const iTaSC::ConstraintValues *values, + unsigned int _nvalues, + IK_Target *iktarget) { - iktarget->blenderConstraint->lin_error = (float)(values->values[0].y - values->values[0].yd); + iktarget->blenderConstraint->lin_error = (float)(values->values[0].y - values->values[0].yd); } -static bool joint_callback(const iTaSC::Timestamp& timestamp, iTaSC::ConstraintValues *const _values, unsigned int _nvalues, void *_param) +static bool joint_callback(const iTaSC::Timestamp ×tamp, + iTaSC::ConstraintValues *const _values, + unsigned int _nvalues, + void *_param) { - IK_Channel *ikchan = (IK_Channel *)_param; - bItasc *ikparam = (bItasc *)ikchan->owner->pose->ikparam; - bPoseChannel *chan = ikchan->pchan; - int dof; - - // a channel can be splitted into multiple joints, so we get called multiple - // times for one channel (this callback is only for 1 joint in the armature) - // the IK_JointTarget structure is shared between multiple joint constraint - // and the target joint values is computed only once, remember this in jointValid - // Don't forget to reset it before each frame - if (!ikchan->jointValid) { - float rmat[3][3]; - - if (chan->rotmode > 0) { - /* euler rotations (will cause gimble lock, but this can be alleviated a bit with rotation orders) */ - eulO_to_mat3(rmat, chan->eul, chan->rotmode); - } - else if (chan->rotmode == ROT_MODE_AXISANGLE) { - /* axis-angle - stored in quaternion data, but not really that great for 3D-changing orientations */ - axis_angle_to_mat3(rmat, &chan->quat[1], chan->quat[0]); - } - else { - /* quats are normalized before use to eliminate scaling issues */ - normalize_qt(chan->quat); - quat_to_mat3(rmat, chan->quat); - } - KDL::Rotation jointRot( - rmat[0][0], rmat[1][0], rmat[2][0], - rmat[0][1], rmat[1][1], rmat[2][1], - rmat[0][2], rmat[1][2], rmat[2][2]); - GetJointRotation(jointRot, ikchan->jointType, ikchan->jointValue); - ikchan->jointValid = 1; - } - // determine which part of jointValue is used for this joint - // closely related to the way the joints are defined - switch (ikchan->jointType & ~IK_TRANSY) { - case IK_XDOF: - case IK_YDOF: - case IK_ZDOF: - dof = 0; - break; - case IK_XDOF | IK_YDOF: - // X + Y - dof = (_values[0].id == iTaSC::Armature::ID_JOINT_RX) ? 0 : 1; - break; - case IK_SWING: - // XZ - dof = 0; - break; - case IK_YDOF | IK_ZDOF: - // Z + Y - dof = (_values[0].id == iTaSC::Armature::ID_JOINT_RZ) ? 0 : 1; - break; - case IK_SWING | IK_YDOF: - // XZ + Y - dof = (_values[0].id == iTaSC::Armature::ID_JOINT_RY) ? 2 : 0; - break; - case IK_REVOLUTE: - dof = 0; - break; - default: - dof = -1; - break; - } - if (dof >= 0) { - for (unsigned int i = 0; i < _nvalues; i++, dof++) { - _values[i].values[0].yd = ikchan->jointValue[dof]; - _values[i].alpha = chan->ikrotweight; - _values[i].feedback = ikparam->feedback; - } - } - return true; + IK_Channel *ikchan = (IK_Channel *)_param; + bItasc *ikparam = (bItasc *)ikchan->owner->pose->ikparam; + bPoseChannel *chan = ikchan->pchan; + int dof; + + // a channel can be splitted into multiple joints, so we get called multiple + // times for one channel (this callback is only for 1 joint in the armature) + // the IK_JointTarget structure is shared between multiple joint constraint + // and the target joint values is computed only once, remember this in jointValid + // Don't forget to reset it before each frame + if (!ikchan->jointValid) { + float rmat[3][3]; + + if (chan->rotmode > 0) { + /* euler rotations (will cause gimble lock, but this can be alleviated a bit with rotation orders) */ + eulO_to_mat3(rmat, chan->eul, chan->rotmode); + } + else if (chan->rotmode == ROT_MODE_AXISANGLE) { + /* axis-angle - stored in quaternion data, but not really that great for 3D-changing orientations */ + axis_angle_to_mat3(rmat, &chan->quat[1], chan->quat[0]); + } + else { + /* quats are normalized before use to eliminate scaling issues */ + normalize_qt(chan->quat); + quat_to_mat3(rmat, chan->quat); + } + KDL::Rotation jointRot(rmat[0][0], + rmat[1][0], + rmat[2][0], + rmat[0][1], + rmat[1][1], + rmat[2][1], + rmat[0][2], + rmat[1][2], + rmat[2][2]); + GetJointRotation(jointRot, ikchan->jointType, ikchan->jointValue); + ikchan->jointValid = 1; + } + // determine which part of jointValue is used for this joint + // closely related to the way the joints are defined + switch (ikchan->jointType & ~IK_TRANSY) { + case IK_XDOF: + case IK_YDOF: + case IK_ZDOF: + dof = 0; + break; + case IK_XDOF | IK_YDOF: + // X + Y + dof = (_values[0].id == iTaSC::Armature::ID_JOINT_RX) ? 0 : 1; + break; + case IK_SWING: + // XZ + dof = 0; + break; + case IK_YDOF | IK_ZDOF: + // Z + Y + dof = (_values[0].id == iTaSC::Armature::ID_JOINT_RZ) ? 0 : 1; + break; + case IK_SWING | IK_YDOF: + // XZ + Y + dof = (_values[0].id == iTaSC::Armature::ID_JOINT_RY) ? 2 : 0; + break; + case IK_REVOLUTE: + dof = 0; + break; + default: + dof = -1; + break; + } + if (dof >= 0) { + for (unsigned int i = 0; i < _nvalues; i++, dof++) { + _values[i].values[0].yd = ikchan->jointValue[dof]; + _values[i].alpha = chan->ikrotweight; + _values[i].feedback = ikparam->feedback; + } + } + return true; } // build array of joint corresponding to IK chain -static int convert_channels(struct Depsgraph *depsgraph, IK_Scene *ikscene, PoseTree *tree, float ctime) +static int convert_channels(struct Depsgraph *depsgraph, + IK_Scene *ikscene, + PoseTree *tree, + float ctime) { - IK_Channel *ikchan; - bPoseChannel *pchan; - int a, flag, njoint; - - njoint = 0; - for (a = 0, ikchan = ikscene->channels; a < ikscene->numchan; ++a, ++ikchan) { - pchan = tree->pchan[a]; - ikchan->pchan = pchan; - ikchan->parent = (a > 0) ? tree->parent[a] : -1; - ikchan->owner = ikscene->blArmature; - - // the constraint and channels must be applied before we build the iTaSC scene, - // this is because some of the pose data (e.g. pose head) don't have corresponding - // joint angles and can't be applied to the iTaSC armature dynamically - if (!(pchan->flag & POSE_DONE)) - BKE_pose_where_is_bone(depsgraph, ikscene->blscene, ikscene->blArmature, pchan, ctime, 1); - // tell blender that this channel was controlled by IK, it's cleared on each BKE_pose_where_is() - pchan->flag |= (POSE_DONE | POSE_CHAIN); - - /* set DoF flag */ - flag = 0; - if (!(pchan->ikflag & BONE_IK_NO_XDOF) && !(pchan->ikflag & BONE_IK_NO_XDOF_TEMP) && - (!(pchan->ikflag & BONE_IK_XLIMIT) || pchan->limitmin[0] < 0.f || pchan->limitmax[0] > 0.f)) - { - flag |= IK_XDOF; - } - if (!(pchan->ikflag & BONE_IK_NO_YDOF) && !(pchan->ikflag & BONE_IK_NO_YDOF_TEMP) && - (!(pchan->ikflag & BONE_IK_YLIMIT) || pchan->limitmin[1] < 0.f || pchan->limitmax[1] > 0.f)) - { - flag |= IK_YDOF; - } - if (!(pchan->ikflag & BONE_IK_NO_ZDOF) && !(pchan->ikflag & BONE_IK_NO_ZDOF_TEMP) && - (!(pchan->ikflag & BONE_IK_ZLIMIT) || pchan->limitmin[2] < 0.f || pchan->limitmax[2] > 0.f)) - { - flag |= IK_ZDOF; - } - - if (tree->stretch && (pchan->ikstretch > 0.0)) { - flag |= IK_TRANSY; - } - /* - * Logic to create the segments: - * RX,RY,RZ = rotational joints with no length - * RY(tip) = rotational joints with a fixed length arm = (0,length,0) - * TY = translational joint on Y axis - * F(pos) = fixed joint with an arm at position pos - * Conversion rule of the above flags: - * - ==> F(tip) - * X ==> RX(tip) - * Y ==> RY(tip) - * Z ==> RZ(tip) - * XY ==> RX+RY(tip) - * XZ ==> RX+RZ(tip) - * YZ ==> RZ+RY(tip) - * XYZ ==> full spherical unless there are limits, in which case RX+RZ+RY(tip) - * In case of stretch, tip=(0,0,0) and there is an additional TY joint - * The frame at last of these joints represents the tail of the bone. - * The head is computed by a reverse translation on Y axis of the bone length - * or in case of TY joint, by the frame at previous joint. - * In case of separation of bones, there is an additional F(head) joint - * - * Computing rest pose and length is complicated: the solver works in world space - * Here is the logic: - * rest position is computed only from bone->bone_mat. - * bone length is computed from bone->length multiplied by the scaling factor of - * the armature. Non-uniform scaling will give bad result! - */ - switch (flag & (IK_XDOF | IK_YDOF | IK_ZDOF)) { - default: - ikchan->jointType = 0; - ikchan->ndof = 0; - break; - case IK_XDOF: - // RX only, get the X rotation - ikchan->jointType = IK_XDOF; - ikchan->ndof = 1; - break; - case IK_YDOF: - // RY only, get the Y rotation - ikchan->jointType = IK_YDOF; - ikchan->ndof = 1; - break; - case IK_ZDOF: - // RZ only, get the Zz rotation - ikchan->jointType = IK_ZDOF; - ikchan->ndof = 1; - break; - case IK_XDOF | IK_YDOF: - ikchan->jointType = IK_XDOF | IK_YDOF; - ikchan->ndof = 2; - break; - case IK_XDOF | IK_ZDOF: - // RX+RZ - ikchan->jointType = IK_SWING; - ikchan->ndof = 2; - break; - case IK_YDOF | IK_ZDOF: - // RZ+RY - ikchan->jointType = IK_ZDOF | IK_YDOF; - ikchan->ndof = 2; - break; - case IK_XDOF | IK_YDOF | IK_ZDOF: - // spherical joint - if (pchan->ikflag & (BONE_IK_XLIMIT | BONE_IK_YLIMIT | BONE_IK_ZLIMIT)) - // decompose in a Swing+RotY joint - ikchan->jointType = IK_SWING | IK_YDOF; - else - ikchan->jointType = IK_REVOLUTE; - ikchan->ndof = 3; - break; - } - if (flag & IK_TRANSY) { - ikchan->jointType |= IK_TRANSY; - ikchan->ndof++; - } - njoint += ikchan->ndof; - } - // njoint is the joint coordinate, create the Joint Array - ikscene->jointArray.resize(njoint); - ikscene->numjoint = njoint; - return njoint; + IK_Channel *ikchan; + bPoseChannel *pchan; + int a, flag, njoint; + + njoint = 0; + for (a = 0, ikchan = ikscene->channels; a < ikscene->numchan; ++a, ++ikchan) { + pchan = tree->pchan[a]; + ikchan->pchan = pchan; + ikchan->parent = (a > 0) ? tree->parent[a] : -1; + ikchan->owner = ikscene->blArmature; + + // the constraint and channels must be applied before we build the iTaSC scene, + // this is because some of the pose data (e.g. pose head) don't have corresponding + // joint angles and can't be applied to the iTaSC armature dynamically + if (!(pchan->flag & POSE_DONE)) + BKE_pose_where_is_bone(depsgraph, ikscene->blscene, ikscene->blArmature, pchan, ctime, 1); + // tell blender that this channel was controlled by IK, it's cleared on each BKE_pose_where_is() + pchan->flag |= (POSE_DONE | POSE_CHAIN); + + /* set DoF flag */ + flag = 0; + if (!(pchan->ikflag & BONE_IK_NO_XDOF) && !(pchan->ikflag & BONE_IK_NO_XDOF_TEMP) && + (!(pchan->ikflag & BONE_IK_XLIMIT) || pchan->limitmin[0] < 0.f || + pchan->limitmax[0] > 0.f)) { + flag |= IK_XDOF; + } + if (!(pchan->ikflag & BONE_IK_NO_YDOF) && !(pchan->ikflag & BONE_IK_NO_YDOF_TEMP) && + (!(pchan->ikflag & BONE_IK_YLIMIT) || pchan->limitmin[1] < 0.f || + pchan->limitmax[1] > 0.f)) { + flag |= IK_YDOF; + } + if (!(pchan->ikflag & BONE_IK_NO_ZDOF) && !(pchan->ikflag & BONE_IK_NO_ZDOF_TEMP) && + (!(pchan->ikflag & BONE_IK_ZLIMIT) || pchan->limitmin[2] < 0.f || + pchan->limitmax[2] > 0.f)) { + flag |= IK_ZDOF; + } + + if (tree->stretch && (pchan->ikstretch > 0.0)) { + flag |= IK_TRANSY; + } + /* + * Logic to create the segments: + * RX,RY,RZ = rotational joints with no length + * RY(tip) = rotational joints with a fixed length arm = (0,length,0) + * TY = translational joint on Y axis + * F(pos) = fixed joint with an arm at position pos + * Conversion rule of the above flags: + * - ==> F(tip) + * X ==> RX(tip) + * Y ==> RY(tip) + * Z ==> RZ(tip) + * XY ==> RX+RY(tip) + * XZ ==> RX+RZ(tip) + * YZ ==> RZ+RY(tip) + * XYZ ==> full spherical unless there are limits, in which case RX+RZ+RY(tip) + * In case of stretch, tip=(0,0,0) and there is an additional TY joint + * The frame at last of these joints represents the tail of the bone. + * The head is computed by a reverse translation on Y axis of the bone length + * or in case of TY joint, by the frame at previous joint. + * In case of separation of bones, there is an additional F(head) joint + * + * Computing rest pose and length is complicated: the solver works in world space + * Here is the logic: + * rest position is computed only from bone->bone_mat. + * bone length is computed from bone->length multiplied by the scaling factor of + * the armature. Non-uniform scaling will give bad result! + */ + switch (flag & (IK_XDOF | IK_YDOF | IK_ZDOF)) { + default: + ikchan->jointType = 0; + ikchan->ndof = 0; + break; + case IK_XDOF: + // RX only, get the X rotation + ikchan->jointType = IK_XDOF; + ikchan->ndof = 1; + break; + case IK_YDOF: + // RY only, get the Y rotation + ikchan->jointType = IK_YDOF; + ikchan->ndof = 1; + break; + case IK_ZDOF: + // RZ only, get the Zz rotation + ikchan->jointType = IK_ZDOF; + ikchan->ndof = 1; + break; + case IK_XDOF | IK_YDOF: + ikchan->jointType = IK_XDOF | IK_YDOF; + ikchan->ndof = 2; + break; + case IK_XDOF | IK_ZDOF: + // RX+RZ + ikchan->jointType = IK_SWING; + ikchan->ndof = 2; + break; + case IK_YDOF | IK_ZDOF: + // RZ+RY + ikchan->jointType = IK_ZDOF | IK_YDOF; + ikchan->ndof = 2; + break; + case IK_XDOF | IK_YDOF | IK_ZDOF: + // spherical joint + if (pchan->ikflag & (BONE_IK_XLIMIT | BONE_IK_YLIMIT | BONE_IK_ZLIMIT)) + // decompose in a Swing+RotY joint + ikchan->jointType = IK_SWING | IK_YDOF; + else + ikchan->jointType = IK_REVOLUTE; + ikchan->ndof = 3; + break; + } + if (flag & IK_TRANSY) { + ikchan->jointType |= IK_TRANSY; + ikchan->ndof++; + } + njoint += ikchan->ndof; + } + // njoint is the joint coordinate, create the Joint Array + ikscene->jointArray.resize(njoint); + ikscene->numjoint = njoint; + return njoint; } // compute array of joint value corresponding to current pose static void convert_pose(IK_Scene *ikscene) { - KDL::Rotation boneRot; - bPoseChannel *pchan; - IK_Channel *ikchan; - Bone *bone; - float rmat[4][4]; // rest pose of bone with parent taken into account - float bmat[4][4]; // difference - float scale; - double *rot; - int a, joint; - - // assume uniform scaling and take Y scale as general scale for the armature - scale = len_v3(ikscene->blArmature->obmat[1]); - rot = ikscene->jointArray(0); - for (joint = a = 0, ikchan = ikscene->channels; a < ikscene->numchan && joint < ikscene->numjoint; ++a, ++ikchan) { - pchan = ikchan->pchan; - bone = pchan->bone; - - if (pchan->parent) { - unit_m4(bmat); - mul_m4_m4m3(bmat, pchan->parent->pose_mat, bone->bone_mat); - } - else { - copy_m4_m4(bmat, bone->arm_mat); - } - invert_m4_m4(rmat, bmat); - mul_m4_m4m4(bmat, rmat, pchan->pose_mat); - normalize_m4(bmat); - boneRot.setValue(bmat[0]); - GetJointRotation(boneRot, ikchan->jointType, rot); - if (ikchan->jointType & IK_TRANSY) { - // compute actual length - rot[ikchan->ndof - 1] = len_v3v3(pchan->pose_tail, pchan->pose_head) * scale; - } - rot += ikchan->ndof; - joint += ikchan->ndof; - } + KDL::Rotation boneRot; + bPoseChannel *pchan; + IK_Channel *ikchan; + Bone *bone; + float rmat[4][4]; // rest pose of bone with parent taken into account + float bmat[4][4]; // difference + float scale; + double *rot; + int a, joint; + + // assume uniform scaling and take Y scale as general scale for the armature + scale = len_v3(ikscene->blArmature->obmat[1]); + rot = ikscene->jointArray(0); + for (joint = a = 0, ikchan = ikscene->channels; + a < ikscene->numchan && joint < ikscene->numjoint; + ++a, ++ikchan) { + pchan = ikchan->pchan; + bone = pchan->bone; + + if (pchan->parent) { + unit_m4(bmat); + mul_m4_m4m3(bmat, pchan->parent->pose_mat, bone->bone_mat); + } + else { + copy_m4_m4(bmat, bone->arm_mat); + } + invert_m4_m4(rmat, bmat); + mul_m4_m4m4(bmat, rmat, pchan->pose_mat); + normalize_m4(bmat); + boneRot.setValue(bmat[0]); + GetJointRotation(boneRot, ikchan->jointType, rot); + if (ikchan->jointType & IK_TRANSY) { + // compute actual length + rot[ikchan->ndof - 1] = len_v3v3(pchan->pose_tail, pchan->pose_head) * scale; + } + rot += ikchan->ndof; + joint += ikchan->ndof; + } } // compute array of joint value corresponding to current pose static void BKE_pose_rest(IK_Scene *ikscene) { - bPoseChannel *pchan; - IK_Channel *ikchan; - Bone *bone; - float scale; - double *rot; - int a, joint; - - // assume uniform scaling and take Y scale as general scale for the armature - scale = len_v3(ikscene->blArmature->obmat[1]); - // rest pose is 0 - SetToZero(ikscene->jointArray); - // except for transY joints - rot = ikscene->jointArray(0); - for (joint = a = 0, ikchan = ikscene->channels; a < ikscene->numchan && joint < ikscene->numjoint; ++a, ++ikchan) { - pchan = ikchan->pchan; - bone = pchan->bone; - - if (ikchan->jointType & IK_TRANSY) - rot[ikchan->ndof - 1] = bone->length * scale; - rot += ikchan->ndof; - joint += ikchan->ndof; - } + bPoseChannel *pchan; + IK_Channel *ikchan; + Bone *bone; + float scale; + double *rot; + int a, joint; + + // assume uniform scaling and take Y scale as general scale for the armature + scale = len_v3(ikscene->blArmature->obmat[1]); + // rest pose is 0 + SetToZero(ikscene->jointArray); + // except for transY joints + rot = ikscene->jointArray(0); + for (joint = a = 0, ikchan = ikscene->channels; + a < ikscene->numchan && joint < ikscene->numjoint; + ++a, ++ikchan) { + pchan = ikchan->pchan; + bone = pchan->bone; + + if (ikchan->jointType & IK_TRANSY) + rot[ikchan->ndof - 1] = bone->length * scale; + rot += ikchan->ndof; + joint += ikchan->ndof; + } } -static IK_Scene *convert_tree(struct Depsgraph *depsgraph, Scene *blscene, Object *ob, bPoseChannel *pchan, float ctime) +static IK_Scene *convert_tree( + struct Depsgraph *depsgraph, Scene *blscene, Object *ob, bPoseChannel *pchan, float ctime) { - PoseTree *tree = (PoseTree *)pchan->iktree.first; - PoseTarget *target; - bKinematicConstraint *condata; - bConstraint *polarcon; - bItasc *ikparam; - iTaSC::Armature *arm; - iTaSC::Scene *scene; - IK_Scene *ikscene; - IK_Channel *ikchan; - KDL::Frame initPose; - Bone *bone; - int a, numtarget; - unsigned int t; - float length; - bool ret = true; - double *rot; - float start[3]; - - if (tree->totchannel == 0) - return NULL; - - ikscene = new IK_Scene; - ikscene->blscene = blscene; - ikscene->bldepsgraph = depsgraph; - arm = new iTaSC::Armature(); - scene = new iTaSC::Scene(); - ikscene->channels = new IK_Channel[tree->totchannel]; - ikscene->numchan = tree->totchannel; - ikscene->armature = arm; - ikscene->scene = scene; - ikparam = (bItasc *)ob->pose->ikparam; - - if (!ikparam) { - // you must have our own copy - ikparam = &DefIKParam; - } - - if (ikparam->flag & ITASC_SIMULATION) - // no cache in animation mode - ikscene->cache = new iTaSC::Cache(); - - switch (ikparam->solver) { - case ITASC_SOLVER_SDLS: - ikscene->solver = new iTaSC::WSDLSSolver(); - break; - case ITASC_SOLVER_DLS: - ikscene->solver = new iTaSC::WDLSSolver(); - break; - default: - delete ikscene; - return NULL; - } - ikscene->blArmature = ob; - // assume uniform scaling and take Y scale as general scale for the armature - ikscene->blScale = len_v3(ob->obmat[1]); - ikscene->blInvScale = (ikscene->blScale < KDL::epsilon) ? 0.0f : 1.0f / ikscene->blScale; - - std::string joint; - std::string root("root"); - std::string parent; - std::vector<double> weights; - double weight[3]; - // build the array of joints corresponding to the IK chain - convert_channels(depsgraph, ikscene, tree, ctime); - // in Blender, the rest pose is always 0 for joints - BKE_pose_rest(ikscene); - rot = ikscene->jointArray(0); - - for (a = 0, ikchan = ikscene->channels; a < tree->totchannel; ++a, ++ikchan) { - pchan = ikchan->pchan; - bone = pchan->bone; - - KDL::Frame tip(iTaSC::F_identity); - // compute the position and rotation of the head from previous segment - Vector3 *fl = bone->bone_mat; - KDL::Rotation brot( - fl[0][0], fl[1][0], fl[2][0], - fl[0][1], fl[1][1], fl[2][1], - fl[0][2], fl[1][2], fl[2][2]); - // if the bone is disconnected, the head is movable in pose mode - // take that into account by using pose matrix instead of bone - // Note that pose is expressed in armature space, convert to previous bone space - { - float R_parmat[3][3]; - float iR_parmat[3][3]; - if (pchan->parent) - copy_m3_m4(R_parmat, pchan->parent->pose_mat); - else - unit_m3(R_parmat); - if (pchan->parent) - sub_v3_v3v3(start, pchan->pose_head, pchan->parent->pose_tail); - else - start[0] = start[1] = start[2] = 0.0f; - invert_m3_m3(iR_parmat, R_parmat); - normalize_m3(iR_parmat); - mul_m3_v3(iR_parmat, start); - } - KDL::Vector bpos(start[0], start[1], start[2]); - bpos *= ikscene->blScale; - KDL::Frame head(brot, bpos); - - // rest pose length of the bone taking scaling into account - length = bone->length * ikscene->blScale; - parent = (a > 0) ? ikscene->channels[tree->parent[a]].tail : root; - // first the fixed segment to the bone head - if (!(ikchan->pchan->bone->flag & BONE_CONNECTED) || head.M.GetRot().Norm() > KDL::epsilon) { - joint = bone->name; - joint += ":H"; - ret = arm->addSegment(joint, parent, KDL::Joint::None, 0.0, head); - parent = joint; - } - if (!(ikchan->jointType & IK_TRANSY)) { - // fixed length, put it in tip - tip.p[1] = length; - } - joint = bone->name; - weight[0] = (1.0 - pchan->stiffness[0]); - weight[1] = (1.0 - pchan->stiffness[1]); - weight[2] = (1.0 - pchan->stiffness[2]); - switch (ikchan->jointType & ~IK_TRANSY) { - case 0: - // fixed bone - joint += ":F"; - ret = arm->addSegment(joint, parent, KDL::Joint::None, 0.0, tip); - break; - case IK_XDOF: - // RX only, get the X rotation - joint += ":RX"; - ret = arm->addSegment(joint, parent, KDL::Joint::RotX, rot[0], tip); - weights.push_back(weight[0]); - break; - case IK_YDOF: - // RY only, get the Y rotation - joint += ":RY"; - ret = arm->addSegment(joint, parent, KDL::Joint::RotY, rot[0], tip); - weights.push_back(weight[1]); - break; - case IK_ZDOF: - // RZ only, get the Zz rotation - joint += ":RZ"; - ret = arm->addSegment(joint, parent, KDL::Joint::RotZ, rot[0], tip); - weights.push_back(weight[2]); - break; - case IK_XDOF | IK_YDOF: - joint += ":RX"; - ret = arm->addSegment(joint, parent, KDL::Joint::RotX, rot[0]); - weights.push_back(weight[0]); - if (ret) { - parent = joint; - joint = bone->name; - joint += ":RY"; - ret = arm->addSegment(joint, parent, KDL::Joint::RotY, rot[1], tip); - weights.push_back(weight[1]); - } - break; - case IK_SWING: - joint += ":SW"; - ret = arm->addSegment(joint, parent, KDL::Joint::Swing, rot[0], tip); - weights.push_back(weight[0]); - weights.push_back(weight[2]); - break; - case IK_YDOF | IK_ZDOF: - // RZ+RY - joint += ":RZ"; - ret = arm->addSegment(joint, parent, KDL::Joint::RotZ, rot[0]); - weights.push_back(weight[2]); - if (ret) { - parent = joint; - joint = bone->name; - joint += ":RY"; - ret = arm->addSegment(joint, parent, KDL::Joint::RotY, rot[1], tip); - weights.push_back(weight[1]); - } - break; - case IK_SWING | IK_YDOF: - // decompose in a Swing+RotY joint - joint += ":SW"; - ret = arm->addSegment(joint, parent, KDL::Joint::Swing, rot[0]); - weights.push_back(weight[0]); - weights.push_back(weight[2]); - if (ret) { - parent = joint; - joint = bone->name; - joint += ":RY"; - ret = arm->addSegment(joint, parent, KDL::Joint::RotY, rot[2], tip); - weights.push_back(weight[1]); - } - break; - case IK_REVOLUTE: - joint += ":SJ"; - ret = arm->addSegment(joint, parent, KDL::Joint::Sphere, rot[0], tip); - weights.push_back(weight[0]); - weights.push_back(weight[1]); - weights.push_back(weight[2]); - break; - } - if (ret && (ikchan->jointType & IK_TRANSY)) { - parent = joint; - joint = bone->name; - joint += ":TY"; - ret = arm->addSegment(joint, parent, KDL::Joint::TransY, rot[ikchan->ndof - 1]); - const float ikstretch = pchan->ikstretch * pchan->ikstretch; - /* why invert twice here? */ - weight[1] = (1.0 - min_ff(1.0 - ikstretch, 1.0f - 0.001f)); - weights.push_back(weight[1]); - } - if (!ret) - // error making the armature?? - break; - // joint points to the segment that correspond to the bone per say - ikchan->tail = joint; - ikchan->head = parent; - // in case of error - ret = false; - if ((ikchan->jointType & IK_XDOF) && (pchan->ikflag & (BONE_IK_XLIMIT | BONE_IK_ROTCTL))) { - joint = bone->name; - joint += ":RX"; - if (pchan->ikflag & BONE_IK_XLIMIT) { - if (arm->addLimitConstraint(joint, 0, pchan->limitmin[0], pchan->limitmax[0]) < 0) - break; - } - if (pchan->ikflag & BONE_IK_ROTCTL) { - if (arm->addConstraint(joint, joint_callback, ikchan, false, false) < 0) - break; - } - } - if ((ikchan->jointType & IK_YDOF) && (pchan->ikflag & (BONE_IK_YLIMIT | BONE_IK_ROTCTL))) { - joint = bone->name; - joint += ":RY"; - if (pchan->ikflag & BONE_IK_YLIMIT) { - if (arm->addLimitConstraint(joint, 0, pchan->limitmin[1], pchan->limitmax[1]) < 0) - break; - } - if (pchan->ikflag & BONE_IK_ROTCTL) { - if (arm->addConstraint(joint, joint_callback, ikchan, false, false) < 0) - break; - } - } - if ((ikchan->jointType & IK_ZDOF) && (pchan->ikflag & (BONE_IK_ZLIMIT | BONE_IK_ROTCTL))) { - joint = bone->name; - joint += ":RZ"; - if (pchan->ikflag & BONE_IK_ZLIMIT) { - if (arm->addLimitConstraint(joint, 0, pchan->limitmin[2], pchan->limitmax[2]) < 0) - break; - } - if (pchan->ikflag & BONE_IK_ROTCTL) { - if (arm->addConstraint(joint, joint_callback, ikchan, false, false) < 0) - break; - } - } - if ((ikchan->jointType & IK_SWING) && (pchan->ikflag & (BONE_IK_XLIMIT | BONE_IK_ZLIMIT | BONE_IK_ROTCTL))) { - joint = bone->name; - joint += ":SW"; - if (pchan->ikflag & BONE_IK_XLIMIT) { - if (arm->addLimitConstraint(joint, 0, pchan->limitmin[0], pchan->limitmax[0]) < 0) - break; - } - if (pchan->ikflag & BONE_IK_ZLIMIT) { - if (arm->addLimitConstraint(joint, 1, pchan->limitmin[2], pchan->limitmax[2]) < 0) - break; - } - if (pchan->ikflag & BONE_IK_ROTCTL) { - if (arm->addConstraint(joint, joint_callback, ikchan, false, false) < 0) - break; - } - } - if ((ikchan->jointType & IK_REVOLUTE) && (pchan->ikflag & BONE_IK_ROTCTL)) { - joint = bone->name; - joint += ":SJ"; - if (arm->addConstraint(joint, joint_callback, ikchan, false, false) < 0) - break; - } - // no error, so restore - ret = true; - rot += ikchan->ndof; - } - if (!ret) { - delete ikscene; - return NULL; - } - // for each target, we need to add an end effector in the armature - for (numtarget = 0, polarcon = NULL, ret = true, target = (PoseTarget *)tree->targets.first; target; target = (PoseTarget *)target->next) { - condata = (bKinematicConstraint *)target->con->data; - pchan = tree->pchan[target->tip]; - - if (is_cartesian_constraint(target->con)) { - // add the end effector - IK_Target *iktarget = new IK_Target(); - ikscene->targets.push_back(iktarget); - iktarget->ee = arm->addEndEffector(ikscene->channels[target->tip].tail); - if (iktarget->ee == -1) { - ret = false; - break; - } - // initialize all the fields that we can set at this time - iktarget->blenderConstraint = target->con; - iktarget->channel = target->tip; - iktarget->simulation = (ikparam->flag & ITASC_SIMULATION); - iktarget->rootChannel = ikscene->channels[0].pchan; - iktarget->owner = ob; - iktarget->targetName = pchan->bone->name; - iktarget->targetName += ":T:"; - iktarget->targetName += target->con->name; - iktarget->constraintName = pchan->bone->name; - iktarget->constraintName += ":C:"; - iktarget->constraintName += target->con->name; - numtarget++; - if (condata->poletar) - // this constraint has a polar target - polarcon = target->con; - } - } - // deal with polar target if any - if (numtarget == 1 && polarcon) { - ikscene->polarConstraint = polarcon; - } - // we can now add the armature - // the armature is based on a moving frame. - // initialize with the correct position in case there is no cache - base_callback(iTaSC::Timestamp(), iTaSC::F_identity, initPose, ikscene); - ikscene->base = new iTaSC::MovingFrame(initPose); - ikscene->base->setCallback(base_callback, ikscene); - std::string armname; - armname = ob->id.name; - armname += ":B"; - ret = scene->addObject(armname, ikscene->base); - armname = ob->id.name; - armname += ":AR"; - if (ret) - ret = scene->addObject(armname, ikscene->armature, ikscene->base); - if (!ret) { - delete ikscene; - return NULL; - } - // set the weight - e_matrix& Wq = arm->getWq(); - assert(Wq.cols() == (int)weights.size()); - for (int q = 0; q < Wq.cols(); q++) - Wq(q, q) = weights[q]; - // get the inverse rest pose frame of the base to compute relative rest pose of end effectors - // this is needed to handle the enforce parameter - // ikscene->pchan[0] is the root channel of the tree - // if it has no parent, then it's just the identify Frame - float invBaseFrame[4][4]; - pchan = ikscene->channels[0].pchan; - if (pchan->parent) { - // it has a parent, get the pose matrix from it - float baseFrame[4][4]; - pchan = pchan->parent; - copy_m4_m4(baseFrame, pchan->bone->arm_mat); - // move to the tail and scale to get rest pose of armature base - copy_v3_v3(baseFrame[3], pchan->bone->arm_tail); - invert_m4_m4(invBaseFrame, baseFrame); - } - else { - unit_m4(invBaseFrame); - } - // finally add the constraint - for (t = 0; t < ikscene->targets.size(); t++) { - IK_Target *iktarget = ikscene->targets[t]; - iktarget->blscene = blscene; - iktarget->bldepsgraph = depsgraph; - condata = (bKinematicConstraint *)iktarget->blenderConstraint->data; - pchan = tree->pchan[iktarget->channel]; - unsigned int controltype, bonecnt; - double bonelen; - float mat[4][4]; - - // add the end effector - // estimate the average bone length, used to clamp feedback error - for (bonecnt = 0, bonelen = 0.f, a = iktarget->channel; a >= 0; a = tree->parent[a], bonecnt++) - bonelen += ikscene->blScale * tree->pchan[a]->bone->length; - bonelen /= bonecnt; - - // store the rest pose of the end effector to compute enforce target - copy_m4_m4(mat, pchan->bone->arm_mat); - copy_v3_v3(mat[3], pchan->bone->arm_tail); - // get the rest pose relative to the armature base - mul_m4_m4m4(iktarget->eeRest, invBaseFrame, mat); - iktarget->eeBlend = (!ikscene->polarConstraint && condata->type == CONSTRAINT_IK_COPYPOSE) ? true : false; - // use target_callback to make sure the initPose includes enforce coefficient - target_callback(iTaSC::Timestamp(), iTaSC::F_identity, initPose, iktarget); - iktarget->target = new iTaSC::MovingFrame(initPose); - iktarget->target->setCallback(target_callback, iktarget); - ret = scene->addObject(iktarget->targetName, iktarget->target); - if (!ret) - break; - - switch (condata->type) { - case CONSTRAINT_IK_COPYPOSE: - controltype = 0; - if (condata->flag & CONSTRAINT_IK_ROT) { - if (!(condata->flag & CONSTRAINT_IK_NO_ROT_X)) - controltype |= iTaSC::CopyPose::CTL_ROTATIONX; - if (!(condata->flag & CONSTRAINT_IK_NO_ROT_Y)) - controltype |= iTaSC::CopyPose::CTL_ROTATIONY; - if (!(condata->flag & CONSTRAINT_IK_NO_ROT_Z)) - controltype |= iTaSC::CopyPose::CTL_ROTATIONZ; - } - if (condata->flag & CONSTRAINT_IK_POS) { - if (!(condata->flag & CONSTRAINT_IK_NO_POS_X)) - controltype |= iTaSC::CopyPose::CTL_POSITIONX; - if (!(condata->flag & CONSTRAINT_IK_NO_POS_Y)) - controltype |= iTaSC::CopyPose::CTL_POSITIONY; - if (!(condata->flag & CONSTRAINT_IK_NO_POS_Z)) - controltype |= iTaSC::CopyPose::CTL_POSITIONZ; - } - if (controltype) { - iktarget->constraint = new iTaSC::CopyPose(controltype, controltype, bonelen); - // set the gain - if (controltype & iTaSC::CopyPose::CTL_POSITION) - iktarget->constraint->setControlParameter(iTaSC::CopyPose::ID_POSITION, iTaSC::ACT_ALPHA, condata->weight); - if (controltype & iTaSC::CopyPose::CTL_ROTATION) - iktarget->constraint->setControlParameter(iTaSC::CopyPose::ID_ROTATION, iTaSC::ACT_ALPHA, condata->orientweight); - iktarget->constraint->registerCallback(copypose_callback, iktarget); - iktarget->errorCallback = copypose_error; - iktarget->controlType = controltype; - // add the constraint - if (condata->flag & CONSTRAINT_IK_TARGETAXIS) - ret = scene->addConstraintSet(iktarget->constraintName, iktarget->constraint, iktarget->targetName, armname, "", ikscene->channels[iktarget->channel].tail); - else - ret = scene->addConstraintSet(iktarget->constraintName, iktarget->constraint, armname, iktarget->targetName, ikscene->channels[iktarget->channel].tail); - } - break; - case CONSTRAINT_IK_DISTANCE: - iktarget->constraint = new iTaSC::Distance(bonelen); - iktarget->constraint->setControlParameter(iTaSC::Distance::ID_DISTANCE, iTaSC::ACT_VALUE, condata->dist); - iktarget->constraint->registerCallback(distance_callback, iktarget); - iktarget->errorCallback = distance_error; - // we can update the weight on each substep - iktarget->constraint->substep(true); - // add the constraint - ret = scene->addConstraintSet(iktarget->constraintName, iktarget->constraint, armname, iktarget->targetName, ikscene->channels[iktarget->channel].tail); - break; - } - if (!ret) - break; - } - if (!ret || - !scene->addCache(ikscene->cache) || - !scene->addSolver(ikscene->solver) || - !scene->initialize()) - { - delete ikscene; - ikscene = NULL; - } - return ikscene; + PoseTree *tree = (PoseTree *)pchan->iktree.first; + PoseTarget *target; + bKinematicConstraint *condata; + bConstraint *polarcon; + bItasc *ikparam; + iTaSC::Armature *arm; + iTaSC::Scene *scene; + IK_Scene *ikscene; + IK_Channel *ikchan; + KDL::Frame initPose; + Bone *bone; + int a, numtarget; + unsigned int t; + float length; + bool ret = true; + double *rot; + float start[3]; + + if (tree->totchannel == 0) + return NULL; + + ikscene = new IK_Scene; + ikscene->blscene = blscene; + ikscene->bldepsgraph = depsgraph; + arm = new iTaSC::Armature(); + scene = new iTaSC::Scene(); + ikscene->channels = new IK_Channel[tree->totchannel]; + ikscene->numchan = tree->totchannel; + ikscene->armature = arm; + ikscene->scene = scene; + ikparam = (bItasc *)ob->pose->ikparam; + + if (!ikparam) { + // you must have our own copy + ikparam = &DefIKParam; + } + + if (ikparam->flag & ITASC_SIMULATION) + // no cache in animation mode + ikscene->cache = new iTaSC::Cache(); + + switch (ikparam->solver) { + case ITASC_SOLVER_SDLS: + ikscene->solver = new iTaSC::WSDLSSolver(); + break; + case ITASC_SOLVER_DLS: + ikscene->solver = new iTaSC::WDLSSolver(); + break; + default: + delete ikscene; + return NULL; + } + ikscene->blArmature = ob; + // assume uniform scaling and take Y scale as general scale for the armature + ikscene->blScale = len_v3(ob->obmat[1]); + ikscene->blInvScale = (ikscene->blScale < KDL::epsilon) ? 0.0f : 1.0f / ikscene->blScale; + + std::string joint; + std::string root("root"); + std::string parent; + std::vector<double> weights; + double weight[3]; + // build the array of joints corresponding to the IK chain + convert_channels(depsgraph, ikscene, tree, ctime); + // in Blender, the rest pose is always 0 for joints + BKE_pose_rest(ikscene); + rot = ikscene->jointArray(0); + + for (a = 0, ikchan = ikscene->channels; a < tree->totchannel; ++a, ++ikchan) { + pchan = ikchan->pchan; + bone = pchan->bone; + + KDL::Frame tip(iTaSC::F_identity); + // compute the position and rotation of the head from previous segment + Vector3 *fl = bone->bone_mat; + KDL::Rotation brot( + fl[0][0], fl[1][0], fl[2][0], fl[0][1], fl[1][1], fl[2][1], fl[0][2], fl[1][2], fl[2][2]); + // if the bone is disconnected, the head is movable in pose mode + // take that into account by using pose matrix instead of bone + // Note that pose is expressed in armature space, convert to previous bone space + { + float R_parmat[3][3]; + float iR_parmat[3][3]; + if (pchan->parent) + copy_m3_m4(R_parmat, pchan->parent->pose_mat); + else + unit_m3(R_parmat); + if (pchan->parent) + sub_v3_v3v3(start, pchan->pose_head, pchan->parent->pose_tail); + else + start[0] = start[1] = start[2] = 0.0f; + invert_m3_m3(iR_parmat, R_parmat); + normalize_m3(iR_parmat); + mul_m3_v3(iR_parmat, start); + } + KDL::Vector bpos(start[0], start[1], start[2]); + bpos *= ikscene->blScale; + KDL::Frame head(brot, bpos); + + // rest pose length of the bone taking scaling into account + length = bone->length * ikscene->blScale; + parent = (a > 0) ? ikscene->channels[tree->parent[a]].tail : root; + // first the fixed segment to the bone head + if (!(ikchan->pchan->bone->flag & BONE_CONNECTED) || head.M.GetRot().Norm() > KDL::epsilon) { + joint = bone->name; + joint += ":H"; + ret = arm->addSegment(joint, parent, KDL::Joint::None, 0.0, head); + parent = joint; + } + if (!(ikchan->jointType & IK_TRANSY)) { + // fixed length, put it in tip + tip.p[1] = length; + } + joint = bone->name; + weight[0] = (1.0 - pchan->stiffness[0]); + weight[1] = (1.0 - pchan->stiffness[1]); + weight[2] = (1.0 - pchan->stiffness[2]); + switch (ikchan->jointType & ~IK_TRANSY) { + case 0: + // fixed bone + joint += ":F"; + ret = arm->addSegment(joint, parent, KDL::Joint::None, 0.0, tip); + break; + case IK_XDOF: + // RX only, get the X rotation + joint += ":RX"; + ret = arm->addSegment(joint, parent, KDL::Joint::RotX, rot[0], tip); + weights.push_back(weight[0]); + break; + case IK_YDOF: + // RY only, get the Y rotation + joint += ":RY"; + ret = arm->addSegment(joint, parent, KDL::Joint::RotY, rot[0], tip); + weights.push_back(weight[1]); + break; + case IK_ZDOF: + // RZ only, get the Zz rotation + joint += ":RZ"; + ret = arm->addSegment(joint, parent, KDL::Joint::RotZ, rot[0], tip); + weights.push_back(weight[2]); + break; + case IK_XDOF | IK_YDOF: + joint += ":RX"; + ret = arm->addSegment(joint, parent, KDL::Joint::RotX, rot[0]); + weights.push_back(weight[0]); + if (ret) { + parent = joint; + joint = bone->name; + joint += ":RY"; + ret = arm->addSegment(joint, parent, KDL::Joint::RotY, rot[1], tip); + weights.push_back(weight[1]); + } + break; + case IK_SWING: + joint += ":SW"; + ret = arm->addSegment(joint, parent, KDL::Joint::Swing, rot[0], tip); + weights.push_back(weight[0]); + weights.push_back(weight[2]); + break; + case IK_YDOF | IK_ZDOF: + // RZ+RY + joint += ":RZ"; + ret = arm->addSegment(joint, parent, KDL::Joint::RotZ, rot[0]); + weights.push_back(weight[2]); + if (ret) { + parent = joint; + joint = bone->name; + joint += ":RY"; + ret = arm->addSegment(joint, parent, KDL::Joint::RotY, rot[1], tip); + weights.push_back(weight[1]); + } + break; + case IK_SWING | IK_YDOF: + // decompose in a Swing+RotY joint + joint += ":SW"; + ret = arm->addSegment(joint, parent, KDL::Joint::Swing, rot[0]); + weights.push_back(weight[0]); + weights.push_back(weight[2]); + if (ret) { + parent = joint; + joint = bone->name; + joint += ":RY"; + ret = arm->addSegment(joint, parent, KDL::Joint::RotY, rot[2], tip); + weights.push_back(weight[1]); + } + break; + case IK_REVOLUTE: + joint += ":SJ"; + ret = arm->addSegment(joint, parent, KDL::Joint::Sphere, rot[0], tip); + weights.push_back(weight[0]); + weights.push_back(weight[1]); + weights.push_back(weight[2]); + break; + } + if (ret && (ikchan->jointType & IK_TRANSY)) { + parent = joint; + joint = bone->name; + joint += ":TY"; + ret = arm->addSegment(joint, parent, KDL::Joint::TransY, rot[ikchan->ndof - 1]); + const float ikstretch = pchan->ikstretch * pchan->ikstretch; + /* why invert twice here? */ + weight[1] = (1.0 - min_ff(1.0 - ikstretch, 1.0f - 0.001f)); + weights.push_back(weight[1]); + } + if (!ret) + // error making the armature?? + break; + // joint points to the segment that correspond to the bone per say + ikchan->tail = joint; + ikchan->head = parent; + // in case of error + ret = false; + if ((ikchan->jointType & IK_XDOF) && (pchan->ikflag & (BONE_IK_XLIMIT | BONE_IK_ROTCTL))) { + joint = bone->name; + joint += ":RX"; + if (pchan->ikflag & BONE_IK_XLIMIT) { + if (arm->addLimitConstraint(joint, 0, pchan->limitmin[0], pchan->limitmax[0]) < 0) + break; + } + if (pchan->ikflag & BONE_IK_ROTCTL) { + if (arm->addConstraint(joint, joint_callback, ikchan, false, false) < 0) + break; + } + } + if ((ikchan->jointType & IK_YDOF) && (pchan->ikflag & (BONE_IK_YLIMIT | BONE_IK_ROTCTL))) { + joint = bone->name; + joint += ":RY"; + if (pchan->ikflag & BONE_IK_YLIMIT) { + if (arm->addLimitConstraint(joint, 0, pchan->limitmin[1], pchan->limitmax[1]) < 0) + break; + } + if (pchan->ikflag & BONE_IK_ROTCTL) { + if (arm->addConstraint(joint, joint_callback, ikchan, false, false) < 0) + break; + } + } + if ((ikchan->jointType & IK_ZDOF) && (pchan->ikflag & (BONE_IK_ZLIMIT | BONE_IK_ROTCTL))) { + joint = bone->name; + joint += ":RZ"; + if (pchan->ikflag & BONE_IK_ZLIMIT) { + if (arm->addLimitConstraint(joint, 0, pchan->limitmin[2], pchan->limitmax[2]) < 0) + break; + } + if (pchan->ikflag & BONE_IK_ROTCTL) { + if (arm->addConstraint(joint, joint_callback, ikchan, false, false) < 0) + break; + } + } + if ((ikchan->jointType & IK_SWING) && + (pchan->ikflag & (BONE_IK_XLIMIT | BONE_IK_ZLIMIT | BONE_IK_ROTCTL))) { + joint = bone->name; + joint += ":SW"; + if (pchan->ikflag & BONE_IK_XLIMIT) { + if (arm->addLimitConstraint(joint, 0, pchan->limitmin[0], pchan->limitmax[0]) < 0) + break; + } + if (pchan->ikflag & BONE_IK_ZLIMIT) { + if (arm->addLimitConstraint(joint, 1, pchan->limitmin[2], pchan->limitmax[2]) < 0) + break; + } + if (pchan->ikflag & BONE_IK_ROTCTL) { + if (arm->addConstraint(joint, joint_callback, ikchan, false, false) < 0) + break; + } + } + if ((ikchan->jointType & IK_REVOLUTE) && (pchan->ikflag & BONE_IK_ROTCTL)) { + joint = bone->name; + joint += ":SJ"; + if (arm->addConstraint(joint, joint_callback, ikchan, false, false) < 0) + break; + } + // no error, so restore + ret = true; + rot += ikchan->ndof; + } + if (!ret) { + delete ikscene; + return NULL; + } + // for each target, we need to add an end effector in the armature + for (numtarget = 0, polarcon = NULL, ret = true, target = (PoseTarget *)tree->targets.first; + target; + target = (PoseTarget *)target->next) { + condata = (bKinematicConstraint *)target->con->data; + pchan = tree->pchan[target->tip]; + + if (is_cartesian_constraint(target->con)) { + // add the end effector + IK_Target *iktarget = new IK_Target(); + ikscene->targets.push_back(iktarget); + iktarget->ee = arm->addEndEffector(ikscene->channels[target->tip].tail); + if (iktarget->ee == -1) { + ret = false; + break; + } + // initialize all the fields that we can set at this time + iktarget->blenderConstraint = target->con; + iktarget->channel = target->tip; + iktarget->simulation = (ikparam->flag & ITASC_SIMULATION); + iktarget->rootChannel = ikscene->channels[0].pchan; + iktarget->owner = ob; + iktarget->targetName = pchan->bone->name; + iktarget->targetName += ":T:"; + iktarget->targetName += target->con->name; + iktarget->constraintName = pchan->bone->name; + iktarget->constraintName += ":C:"; + iktarget->constraintName += target->con->name; + numtarget++; + if (condata->poletar) + // this constraint has a polar target + polarcon = target->con; + } + } + // deal with polar target if any + if (numtarget == 1 && polarcon) { + ikscene->polarConstraint = polarcon; + } + // we can now add the armature + // the armature is based on a moving frame. + // initialize with the correct position in case there is no cache + base_callback(iTaSC::Timestamp(), iTaSC::F_identity, initPose, ikscene); + ikscene->base = new iTaSC::MovingFrame(initPose); + ikscene->base->setCallback(base_callback, ikscene); + std::string armname; + armname = ob->id.name; + armname += ":B"; + ret = scene->addObject(armname, ikscene->base); + armname = ob->id.name; + armname += ":AR"; + if (ret) + ret = scene->addObject(armname, ikscene->armature, ikscene->base); + if (!ret) { + delete ikscene; + return NULL; + } + // set the weight + e_matrix &Wq = arm->getWq(); + assert(Wq.cols() == (int)weights.size()); + for (int q = 0; q < Wq.cols(); q++) + Wq(q, q) = weights[q]; + // get the inverse rest pose frame of the base to compute relative rest pose of end effectors + // this is needed to handle the enforce parameter + // ikscene->pchan[0] is the root channel of the tree + // if it has no parent, then it's just the identify Frame + float invBaseFrame[4][4]; + pchan = ikscene->channels[0].pchan; + if (pchan->parent) { + // it has a parent, get the pose matrix from it + float baseFrame[4][4]; + pchan = pchan->parent; + copy_m4_m4(baseFrame, pchan->bone->arm_mat); + // move to the tail and scale to get rest pose of armature base + copy_v3_v3(baseFrame[3], pchan->bone->arm_tail); + invert_m4_m4(invBaseFrame, baseFrame); + } + else { + unit_m4(invBaseFrame); + } + // finally add the constraint + for (t = 0; t < ikscene->targets.size(); t++) { + IK_Target *iktarget = ikscene->targets[t]; + iktarget->blscene = blscene; + iktarget->bldepsgraph = depsgraph; + condata = (bKinematicConstraint *)iktarget->blenderConstraint->data; + pchan = tree->pchan[iktarget->channel]; + unsigned int controltype, bonecnt; + double bonelen; + float mat[4][4]; + + // add the end effector + // estimate the average bone length, used to clamp feedback error + for (bonecnt = 0, bonelen = 0.f, a = iktarget->channel; a >= 0; a = tree->parent[a], bonecnt++) + bonelen += ikscene->blScale * tree->pchan[a]->bone->length; + bonelen /= bonecnt; + + // store the rest pose of the end effector to compute enforce target + copy_m4_m4(mat, pchan->bone->arm_mat); + copy_v3_v3(mat[3], pchan->bone->arm_tail); + // get the rest pose relative to the armature base + mul_m4_m4m4(iktarget->eeRest, invBaseFrame, mat); + iktarget->eeBlend = (!ikscene->polarConstraint && condata->type == CONSTRAINT_IK_COPYPOSE) ? + true : + false; + // use target_callback to make sure the initPose includes enforce coefficient + target_callback(iTaSC::Timestamp(), iTaSC::F_identity, initPose, iktarget); + iktarget->target = new iTaSC::MovingFrame(initPose); + iktarget->target->setCallback(target_callback, iktarget); + ret = scene->addObject(iktarget->targetName, iktarget->target); + if (!ret) + break; + + switch (condata->type) { + case CONSTRAINT_IK_COPYPOSE: + controltype = 0; + if (condata->flag & CONSTRAINT_IK_ROT) { + if (!(condata->flag & CONSTRAINT_IK_NO_ROT_X)) + controltype |= iTaSC::CopyPose::CTL_ROTATIONX; + if (!(condata->flag & CONSTRAINT_IK_NO_ROT_Y)) + controltype |= iTaSC::CopyPose::CTL_ROTATIONY; + if (!(condata->flag & CONSTRAINT_IK_NO_ROT_Z)) + controltype |= iTaSC::CopyPose::CTL_ROTATIONZ; + } + if (condata->flag & CONSTRAINT_IK_POS) { + if (!(condata->flag & CONSTRAINT_IK_NO_POS_X)) + controltype |= iTaSC::CopyPose::CTL_POSITIONX; + if (!(condata->flag & CONSTRAINT_IK_NO_POS_Y)) + controltype |= iTaSC::CopyPose::CTL_POSITIONY; + if (!(condata->flag & CONSTRAINT_IK_NO_POS_Z)) + controltype |= iTaSC::CopyPose::CTL_POSITIONZ; + } + if (controltype) { + iktarget->constraint = new iTaSC::CopyPose(controltype, controltype, bonelen); + // set the gain + if (controltype & iTaSC::CopyPose::CTL_POSITION) + iktarget->constraint->setControlParameter( + iTaSC::CopyPose::ID_POSITION, iTaSC::ACT_ALPHA, condata->weight); + if (controltype & iTaSC::CopyPose::CTL_ROTATION) + iktarget->constraint->setControlParameter( + iTaSC::CopyPose::ID_ROTATION, iTaSC::ACT_ALPHA, condata->orientweight); + iktarget->constraint->registerCallback(copypose_callback, iktarget); + iktarget->errorCallback = copypose_error; + iktarget->controlType = controltype; + // add the constraint + if (condata->flag & CONSTRAINT_IK_TARGETAXIS) + ret = scene->addConstraintSet(iktarget->constraintName, + iktarget->constraint, + iktarget->targetName, + armname, + "", + ikscene->channels[iktarget->channel].tail); + else + ret = scene->addConstraintSet(iktarget->constraintName, + iktarget->constraint, + armname, + iktarget->targetName, + ikscene->channels[iktarget->channel].tail); + } + break; + case CONSTRAINT_IK_DISTANCE: + iktarget->constraint = new iTaSC::Distance(bonelen); + iktarget->constraint->setControlParameter( + iTaSC::Distance::ID_DISTANCE, iTaSC::ACT_VALUE, condata->dist); + iktarget->constraint->registerCallback(distance_callback, iktarget); + iktarget->errorCallback = distance_error; + // we can update the weight on each substep + iktarget->constraint->substep(true); + // add the constraint + ret = scene->addConstraintSet(iktarget->constraintName, + iktarget->constraint, + armname, + iktarget->targetName, + ikscene->channels[iktarget->channel].tail); + break; + } + if (!ret) + break; + } + if (!ret || !scene->addCache(ikscene->cache) || !scene->addSolver(ikscene->solver) || + !scene->initialize()) { + delete ikscene; + ikscene = NULL; + } + return ikscene; } static void create_scene(struct Depsgraph *depsgraph, Scene *scene, Object *ob, float ctime) { - bPoseChannel *pchan; - - // create the IK scene - for (pchan = (bPoseChannel *)ob->pose->chanbase.first; pchan; pchan = (bPoseChannel *)pchan->next) { - // by construction there is only one tree - PoseTree *tree = (PoseTree *)pchan->iktree.first; - if (tree) { - IK_Data *ikdata = get_ikdata(ob->pose); - // convert tree in iTaSC::Scene - IK_Scene *ikscene = convert_tree(depsgraph, scene, ob, pchan, ctime); - if (ikscene) { - ikscene->next = ikdata->first; - ikdata->first = ikscene; - } - // delete the trees once we are done - while (tree) { - BLI_remlink(&pchan->iktree, tree); - BLI_freelistN(&tree->targets); - if (tree->pchan) MEM_freeN(tree->pchan); - if (tree->parent) MEM_freeN(tree->parent); - if (tree->basis_change) MEM_freeN(tree->basis_change); - MEM_freeN(tree); - tree = (PoseTree *)pchan->iktree.first; - } - } - } + bPoseChannel *pchan; + + // create the IK scene + for (pchan = (bPoseChannel *)ob->pose->chanbase.first; pchan; + pchan = (bPoseChannel *)pchan->next) { + // by construction there is only one tree + PoseTree *tree = (PoseTree *)pchan->iktree.first; + if (tree) { + IK_Data *ikdata = get_ikdata(ob->pose); + // convert tree in iTaSC::Scene + IK_Scene *ikscene = convert_tree(depsgraph, scene, ob, pchan, ctime); + if (ikscene) { + ikscene->next = ikdata->first; + ikdata->first = ikscene; + } + // delete the trees once we are done + while (tree) { + BLI_remlink(&pchan->iktree, tree); + BLI_freelistN(&tree->targets); + if (tree->pchan) + MEM_freeN(tree->pchan); + if (tree->parent) + MEM_freeN(tree->parent); + if (tree->basis_change) + MEM_freeN(tree->basis_change); + MEM_freeN(tree); + tree = (PoseTree *)pchan->iktree.first; + } + } + } } /* returns 1 if scaling has changed and tree must be reinitialized */ static int init_scene(Object *ob) { - // check also if scaling has changed - float scale = len_v3(ob->obmat[1]); - IK_Scene *scene; - - if (ob->pose->ikdata) { - for (scene = ((IK_Data *)ob->pose->ikdata)->first; - scene != NULL; - scene = scene->next) - { - if (fabs(scene->blScale - scale) > KDL::epsilon) - return 1; - scene->channels[0].pchan->flag |= POSE_IKTREE; - } - } - return 0; + // check also if scaling has changed + float scale = len_v3(ob->obmat[1]); + IK_Scene *scene; + + if (ob->pose->ikdata) { + for (scene = ((IK_Data *)ob->pose->ikdata)->first; scene != NULL; scene = scene->next) { + if (fabs(scene->blScale - scale) > KDL::epsilon) + return 1; + scene->channels[0].pchan->flag |= POSE_IKTREE; + } + } + return 0; } -static void execute_scene(struct Depsgraph *depsgraph, Scene *blscene, IK_Scene *ikscene, bItasc *ikparam, float ctime, float frtime) +static void execute_scene(struct Depsgraph *depsgraph, + Scene *blscene, + IK_Scene *ikscene, + bItasc *ikparam, + float ctime, + float frtime) { - int i; - IK_Channel *ikchan; - if (ikparam->flag & ITASC_SIMULATION) { - for (i = 0, ikchan = ikscene->channels; i < ikscene->numchan; i++, ++ikchan) { - // In simulation mode we don't allow external contraint to change our bones, mark the channel done - // also tell Blender that this channel is part of IK tree (cleared on each BKE_pose_where_is() - ikchan->pchan->flag |= (POSE_DONE | POSE_CHAIN); - ikchan->jointValid = 0; - } - } - else { - // in animation mode, we must get the bone position from action and constraints - for (i = 0, ikchan = ikscene->channels; i < ikscene->numchan; i++, ++ikchan) { - if (!(ikchan->pchan->flag & POSE_DONE)) - BKE_pose_where_is_bone(depsgraph, blscene, ikscene->blArmature, ikchan->pchan, ctime, 1); - // tell blender that this channel was controlled by IK, it's cleared on each BKE_pose_where_is() - ikchan->pchan->flag |= (POSE_DONE | POSE_CHAIN); - ikchan->jointValid = 0; - } - } - // only run execute the scene if at least one of our target is enabled - for (i = ikscene->targets.size(); i > 0; --i) { - IK_Target *iktarget = ikscene->targets[i - 1]; - if (!(iktarget->blenderConstraint->flag & CONSTRAINT_OFF)) - break; - } - if (i == 0 && ikscene->armature->getNrOfConstraints() == 0) - // all constraint disabled - return; - - // compute timestep - double timestamp = ctime * frtime + 2147483.648; - double timestep = frtime; - bool reiterate = (ikparam->flag & ITASC_REITERATION) ? true : false; - int numstep = (ikparam->flag & ITASC_AUTO_STEP) ? 0 : ikparam->numstep; - bool simulation = true; - - if (ikparam->flag & ITASC_SIMULATION) { - ikscene->solver->setParam(iTaSC::Solver::DLS_QMAX, ikparam->maxvel); - } - else { - // in animation mode we start from the pose after action and constraint - convert_pose(ikscene); - ikscene->armature->setJointArray(ikscene->jointArray); - // and we don't handle velocity - reiterate = true; - simulation = false; - // time is virtual, so take fixed value for velocity parameters (see itasc_update_param) - // and choose 1s timestep to allow having velocity parameters in radiant - timestep = 1.0; - // use auto setup to let the solver test the variation of the joints - numstep = 0; - } - - if (ikscene->cache && !reiterate && simulation) { - iTaSC::CacheTS sts, cts; - sts = cts = (iTaSC::CacheTS)(timestamp * 1000.0 + 0.5); - if (ikscene->cache->getPreviousCacheItem(ikscene->armature, 0, &cts) == NULL || cts == 0) { - // the cache is empty before this time, reiterate - if (ikparam->flag & ITASC_INITIAL_REITERATION) - reiterate = true; - } - else { - // can take the cache as a start point. - sts -= cts; - timestep = sts / 1000.0; - } - } - // don't cache if we are reiterating because we don't want to destroy the cache unnecessarily - ikscene->scene->update(timestamp, timestep, numstep, false, !reiterate, simulation); - if (reiterate) { - // how many times do we reiterate? - for (i = 0; i < ikparam->numiter; i++) { - if (ikscene->armature->getMaxJointChange() < ikparam->precision || - ikscene->armature->getMaxEndEffectorChange() < ikparam->precision) - { - break; - } - ikscene->scene->update(timestamp, timestep, numstep, true, false, simulation); - } - if (simulation) { - // one more fake iteration to cache - ikscene->scene->update(timestamp, 0.0, 1, true, true, true); - } - } - // compute constraint error - for (i = ikscene->targets.size(); i > 0; --i) { - IK_Target *iktarget = ikscene->targets[i - 1]; - if (!(iktarget->blenderConstraint->flag & CONSTRAINT_OFF) && iktarget->constraint) { - unsigned int nvalues; - const iTaSC::ConstraintValues *values; - values = iktarget->constraint->getControlParameters(&nvalues); - iktarget->errorCallback(values, nvalues, iktarget); - } - } - // Apply result to bone: - // walk the ikscene->channels - // for each, get the Frame of the joint corresponding to the bone relative to its parent - // combine the parent and the joint frame to get the frame relative to armature - // a backward translation of the bone length gives the head - // if TY, compute the scale as the ratio of the joint length with rest pose length - iTaSC::Armature *arm = ikscene->armature; - KDL::Frame frame; - double q_rest[3], q[3]; - const KDL::Joint *joint; - const KDL::Frame *tip; - bPoseChannel *pchan; - float scale; - float length; - float yaxis[3]; - for (i = 0, ikchan = ikscene->channels; i < ikscene->numchan; ++i, ++ikchan) { - if (i == 0) { - if (!arm->getRelativeFrame(frame, ikchan->tail)) - break; - // this frame is relative to base, make it relative to object - ikchan->frame = ikscene->baseFrame * frame; - } - else { - if (!arm->getRelativeFrame(frame, ikchan->tail, ikscene->channels[ikchan->parent].tail)) - break; - // combine with parent frame to get frame relative to object - ikchan->frame = ikscene->channels[ikchan->parent].frame * frame; - } - // ikchan->frame is the tail frame relative to object - // get bone length - if (!arm->getSegment(ikchan->tail, 3, joint, q_rest[0], q[0], tip)) - break; - if (joint->getType() == KDL::Joint::TransY) { - // stretch bones have a TY joint, compute the scale - scale = (float)(q[0] / q_rest[0]); - // the length is the joint itself - length = (float)q[0]; - } - else { - scale = 1.0f; - // for fixed bone, the length is in the tip (always along Y axis) - length = tip->p(1); - } - // ready to compute the pose mat - pchan = ikchan->pchan; - // tail mat - ikchan->frame.getValue(&pchan->pose_mat[0][0]); - // the scale of the object was included in the ik scene, take it out now - // because the pose channels are relative to the object - mul_v3_fl(pchan->pose_mat[3], ikscene->blInvScale); - length *= ikscene->blInvScale; - copy_v3_v3(pchan->pose_tail, pchan->pose_mat[3]); - // shift to head - copy_v3_v3(yaxis, pchan->pose_mat[1]); - mul_v3_fl(yaxis, length); - sub_v3_v3v3(pchan->pose_mat[3], pchan->pose_mat[3], yaxis); - copy_v3_v3(pchan->pose_head, pchan->pose_mat[3]); - // add scale - mul_v3_fl(pchan->pose_mat[0], scale); - mul_v3_fl(pchan->pose_mat[1], scale); - mul_v3_fl(pchan->pose_mat[2], scale); - } - if (i < ikscene->numchan) { - // big problem - ; - } + int i; + IK_Channel *ikchan; + if (ikparam->flag & ITASC_SIMULATION) { + for (i = 0, ikchan = ikscene->channels; i < ikscene->numchan; i++, ++ikchan) { + // In simulation mode we don't allow external contraint to change our bones, mark the channel done + // also tell Blender that this channel is part of IK tree (cleared on each BKE_pose_where_is() + ikchan->pchan->flag |= (POSE_DONE | POSE_CHAIN); + ikchan->jointValid = 0; + } + } + else { + // in animation mode, we must get the bone position from action and constraints + for (i = 0, ikchan = ikscene->channels; i < ikscene->numchan; i++, ++ikchan) { + if (!(ikchan->pchan->flag & POSE_DONE)) + BKE_pose_where_is_bone(depsgraph, blscene, ikscene->blArmature, ikchan->pchan, ctime, 1); + // tell blender that this channel was controlled by IK, it's cleared on each BKE_pose_where_is() + ikchan->pchan->flag |= (POSE_DONE | POSE_CHAIN); + ikchan->jointValid = 0; + } + } + // only run execute the scene if at least one of our target is enabled + for (i = ikscene->targets.size(); i > 0; --i) { + IK_Target *iktarget = ikscene->targets[i - 1]; + if (!(iktarget->blenderConstraint->flag & CONSTRAINT_OFF)) + break; + } + if (i == 0 && ikscene->armature->getNrOfConstraints() == 0) + // all constraint disabled + return; + + // compute timestep + double timestamp = ctime * frtime + 2147483.648; + double timestep = frtime; + bool reiterate = (ikparam->flag & ITASC_REITERATION) ? true : false; + int numstep = (ikparam->flag & ITASC_AUTO_STEP) ? 0 : ikparam->numstep; + bool simulation = true; + + if (ikparam->flag & ITASC_SIMULATION) { + ikscene->solver->setParam(iTaSC::Solver::DLS_QMAX, ikparam->maxvel); + } + else { + // in animation mode we start from the pose after action and constraint + convert_pose(ikscene); + ikscene->armature->setJointArray(ikscene->jointArray); + // and we don't handle velocity + reiterate = true; + simulation = false; + // time is virtual, so take fixed value for velocity parameters (see itasc_update_param) + // and choose 1s timestep to allow having velocity parameters in radiant + timestep = 1.0; + // use auto setup to let the solver test the variation of the joints + numstep = 0; + } + + if (ikscene->cache && !reiterate && simulation) { + iTaSC::CacheTS sts, cts; + sts = cts = (iTaSC::CacheTS)(timestamp * 1000.0 + 0.5); + if (ikscene->cache->getPreviousCacheItem(ikscene->armature, 0, &cts) == NULL || cts == 0) { + // the cache is empty before this time, reiterate + if (ikparam->flag & ITASC_INITIAL_REITERATION) + reiterate = true; + } + else { + // can take the cache as a start point. + sts -= cts; + timestep = sts / 1000.0; + } + } + // don't cache if we are reiterating because we don't want to destroy the cache unnecessarily + ikscene->scene->update(timestamp, timestep, numstep, false, !reiterate, simulation); + if (reiterate) { + // how many times do we reiterate? + for (i = 0; i < ikparam->numiter; i++) { + if (ikscene->armature->getMaxJointChange() < ikparam->precision || + ikscene->armature->getMaxEndEffectorChange() < ikparam->precision) { + break; + } + ikscene->scene->update(timestamp, timestep, numstep, true, false, simulation); + } + if (simulation) { + // one more fake iteration to cache + ikscene->scene->update(timestamp, 0.0, 1, true, true, true); + } + } + // compute constraint error + for (i = ikscene->targets.size(); i > 0; --i) { + IK_Target *iktarget = ikscene->targets[i - 1]; + if (!(iktarget->blenderConstraint->flag & CONSTRAINT_OFF) && iktarget->constraint) { + unsigned int nvalues; + const iTaSC::ConstraintValues *values; + values = iktarget->constraint->getControlParameters(&nvalues); + iktarget->errorCallback(values, nvalues, iktarget); + } + } + // Apply result to bone: + // walk the ikscene->channels + // for each, get the Frame of the joint corresponding to the bone relative to its parent + // combine the parent and the joint frame to get the frame relative to armature + // a backward translation of the bone length gives the head + // if TY, compute the scale as the ratio of the joint length with rest pose length + iTaSC::Armature *arm = ikscene->armature; + KDL::Frame frame; + double q_rest[3], q[3]; + const KDL::Joint *joint; + const KDL::Frame *tip; + bPoseChannel *pchan; + float scale; + float length; + float yaxis[3]; + for (i = 0, ikchan = ikscene->channels; i < ikscene->numchan; ++i, ++ikchan) { + if (i == 0) { + if (!arm->getRelativeFrame(frame, ikchan->tail)) + break; + // this frame is relative to base, make it relative to object + ikchan->frame = ikscene->baseFrame * frame; + } + else { + if (!arm->getRelativeFrame(frame, ikchan->tail, ikscene->channels[ikchan->parent].tail)) + break; + // combine with parent frame to get frame relative to object + ikchan->frame = ikscene->channels[ikchan->parent].frame * frame; + } + // ikchan->frame is the tail frame relative to object + // get bone length + if (!arm->getSegment(ikchan->tail, 3, joint, q_rest[0], q[0], tip)) + break; + if (joint->getType() == KDL::Joint::TransY) { + // stretch bones have a TY joint, compute the scale + scale = (float)(q[0] / q_rest[0]); + // the length is the joint itself + length = (float)q[0]; + } + else { + scale = 1.0f; + // for fixed bone, the length is in the tip (always along Y axis) + length = tip->p(1); + } + // ready to compute the pose mat + pchan = ikchan->pchan; + // tail mat + ikchan->frame.getValue(&pchan->pose_mat[0][0]); + // the scale of the object was included in the ik scene, take it out now + // because the pose channels are relative to the object + mul_v3_fl(pchan->pose_mat[3], ikscene->blInvScale); + length *= ikscene->blInvScale; + copy_v3_v3(pchan->pose_tail, pchan->pose_mat[3]); + // shift to head + copy_v3_v3(yaxis, pchan->pose_mat[1]); + mul_v3_fl(yaxis, length); + sub_v3_v3v3(pchan->pose_mat[3], pchan->pose_mat[3], yaxis); + copy_v3_v3(pchan->pose_head, pchan->pose_mat[3]); + // add scale + mul_v3_fl(pchan->pose_mat[0], scale); + mul_v3_fl(pchan->pose_mat[1], scale); + mul_v3_fl(pchan->pose_mat[2], scale); + } + if (i < ikscene->numchan) { + // big problem + ; + } } //--------------------------------------------------- // plugin interface // -void itasc_initialize_tree(struct Depsgraph *depsgraph, struct Scene *scene, Object *ob, float ctime) +void itasc_initialize_tree(struct Depsgraph *depsgraph, + struct Scene *scene, + Object *ob, + float ctime) { - bPoseChannel *pchan; - int count = 0; - - if (ob->pose->ikdata != NULL && !(ob->pose->flag & POSE_WAS_REBUILT)) { - if (!init_scene(ob)) - return; - } - // first remove old scene - itasc_clear_data(ob->pose); - // we should handle all the constraint and mark them all disabled - // for blender but we'll start with the IK constraint alone - for (pchan = (bPoseChannel *)ob->pose->chanbase.first; pchan; pchan = (bPoseChannel *)pchan->next) { - if (pchan->constflag & PCHAN_HAS_IK) - count += initialize_scene(ob, pchan); - } - // if at least one tree, create the scenes from the PoseTree stored in the channels - // postpone until execute_tree: this way the pose constraint are included - if (count) - create_scene(depsgraph, scene, ob, ctime); - itasc_update_param(ob->pose); - // make sure we don't rebuilt until the user changes something important - ob->pose->flag &= ~POSE_WAS_REBUILT; + bPoseChannel *pchan; + int count = 0; + + if (ob->pose->ikdata != NULL && !(ob->pose->flag & POSE_WAS_REBUILT)) { + if (!init_scene(ob)) + return; + } + // first remove old scene + itasc_clear_data(ob->pose); + // we should handle all the constraint and mark them all disabled + // for blender but we'll start with the IK constraint alone + for (pchan = (bPoseChannel *)ob->pose->chanbase.first; pchan; + pchan = (bPoseChannel *)pchan->next) { + if (pchan->constflag & PCHAN_HAS_IK) + count += initialize_scene(ob, pchan); + } + // if at least one tree, create the scenes from the PoseTree stored in the channels + // postpone until execute_tree: this way the pose constraint are included + if (count) + create_scene(depsgraph, scene, ob, ctime); + itasc_update_param(ob->pose); + // make sure we don't rebuilt until the user changes something important + ob->pose->flag &= ~POSE_WAS_REBUILT; } -void itasc_execute_tree(struct Depsgraph *depsgraph, struct Scene *scene, Object *ob, bPoseChannel *pchan_root, float ctime) +void itasc_execute_tree(struct Depsgraph *depsgraph, + struct Scene *scene, + Object *ob, + bPoseChannel *pchan_root, + float ctime) { - if (ob->pose->ikdata) { - IK_Data *ikdata = (IK_Data *)ob->pose->ikdata; - bItasc *ikparam = (bItasc *) ob->pose->ikparam; - // we need default parameters - if (!ikparam) ikparam = &DefIKParam; - - for (IK_Scene *ikscene = ikdata->first; ikscene; ikscene = ikscene->next) { - if (ikscene->channels[0].pchan == pchan_root) { - float timestep = scene->r.frs_sec_base / scene->r.frs_sec; - execute_scene(depsgraph, scene, ikscene, ikparam, ctime, timestep); - break; - } - } - } + if (ob->pose->ikdata) { + IK_Data *ikdata = (IK_Data *)ob->pose->ikdata; + bItasc *ikparam = (bItasc *)ob->pose->ikparam; + // we need default parameters + if (!ikparam) + ikparam = &DefIKParam; + + for (IK_Scene *ikscene = ikdata->first; ikscene; ikscene = ikscene->next) { + if (ikscene->channels[0].pchan == pchan_root) { + float timestep = scene->r.frs_sec_base / scene->r.frs_sec; + execute_scene(depsgraph, scene, ikscene, ikparam, ctime, timestep); + break; + } + } + } } -void itasc_release_tree(struct Scene *scene, struct Object *ob, float ctime) +void itasc_release_tree(struct Scene *scene, struct Object *ob, float ctime) { - // not used for iTaSC + // not used for iTaSC } void itasc_clear_data(struct bPose *pose) { - if (pose->ikdata) { - IK_Data *ikdata = (IK_Data *)pose->ikdata; - for (IK_Scene *scene = ikdata->first; scene; scene = ikdata->first) { - ikdata->first = scene->next; - delete scene; - } - MEM_freeN(ikdata); - pose->ikdata = NULL; - } + if (pose->ikdata) { + IK_Data *ikdata = (IK_Data *)pose->ikdata; + for (IK_Scene *scene = ikdata->first; scene; scene = ikdata->first) { + ikdata->first = scene->next; + delete scene; + } + MEM_freeN(ikdata); + pose->ikdata = NULL; + } } void itasc_clear_cache(struct bPose *pose) { - if (pose->ikdata) { - IK_Data *ikdata = (IK_Data *)pose->ikdata; - for (IK_Scene *scene = ikdata->first; scene; scene = scene->next) { - if (scene->cache) - // clear all cache but leaving the timestamp 0 (=rest pose) - scene->cache->clearCacheFrom(NULL, 1); - } - } + if (pose->ikdata) { + IK_Data *ikdata = (IK_Data *)pose->ikdata; + for (IK_Scene *scene = ikdata->first; scene; scene = scene->next) { + if (scene->cache) + // clear all cache but leaving the timestamp 0 (=rest pose) + scene->cache->clearCacheFrom(NULL, 1); + } + } } void itasc_update_param(struct bPose *pose) { - if (pose->ikdata && pose->ikparam) { - IK_Data *ikdata = (IK_Data *)pose->ikdata; - bItasc *ikparam = (bItasc *)pose->ikparam; - for (IK_Scene *ikscene = ikdata->first; ikscene; ikscene = ikscene->next) { - double armlength = ikscene->armature->getArmLength(); - ikscene->solver->setParam(iTaSC::Solver::DLS_LAMBDA_MAX, ikparam->dampmax * armlength); - ikscene->solver->setParam(iTaSC::Solver::DLS_EPSILON, ikparam->dampeps * armlength); - if (ikparam->flag & ITASC_SIMULATION) { - ikscene->scene->setParam(iTaSC::Scene::MIN_TIMESTEP, ikparam->minstep); - ikscene->scene->setParam(iTaSC::Scene::MAX_TIMESTEP, ikparam->maxstep); - ikscene->solver->setParam(iTaSC::Solver::DLS_QMAX, ikparam->maxvel); - ikscene->armature->setControlParameter(CONSTRAINT_ID_ALL, iTaSC::Armature::ID_JOINT, iTaSC::ACT_FEEDBACK, ikparam->feedback); - } - else { - // in animation mode timestep is 1s by convention => - // qmax becomes radiant and feedback becomes fraction of error gap corrected in one iteration - ikscene->scene->setParam(iTaSC::Scene::MIN_TIMESTEP, 1.0); - ikscene->scene->setParam(iTaSC::Scene::MAX_TIMESTEP, 1.0); - ikscene->solver->setParam(iTaSC::Solver::DLS_QMAX, 0.52); - ikscene->armature->setControlParameter(CONSTRAINT_ID_ALL, iTaSC::Armature::ID_JOINT, iTaSC::ACT_FEEDBACK, 0.8); - } - } - } + if (pose->ikdata && pose->ikparam) { + IK_Data *ikdata = (IK_Data *)pose->ikdata; + bItasc *ikparam = (bItasc *)pose->ikparam; + for (IK_Scene *ikscene = ikdata->first; ikscene; ikscene = ikscene->next) { + double armlength = ikscene->armature->getArmLength(); + ikscene->solver->setParam(iTaSC::Solver::DLS_LAMBDA_MAX, ikparam->dampmax * armlength); + ikscene->solver->setParam(iTaSC::Solver::DLS_EPSILON, ikparam->dampeps * armlength); + if (ikparam->flag & ITASC_SIMULATION) { + ikscene->scene->setParam(iTaSC::Scene::MIN_TIMESTEP, ikparam->minstep); + ikscene->scene->setParam(iTaSC::Scene::MAX_TIMESTEP, ikparam->maxstep); + ikscene->solver->setParam(iTaSC::Solver::DLS_QMAX, ikparam->maxvel); + ikscene->armature->setControlParameter( + CONSTRAINT_ID_ALL, iTaSC::Armature::ID_JOINT, iTaSC::ACT_FEEDBACK, ikparam->feedback); + } + else { + // in animation mode timestep is 1s by convention => + // qmax becomes radiant and feedback becomes fraction of error gap corrected in one iteration + ikscene->scene->setParam(iTaSC::Scene::MIN_TIMESTEP, 1.0); + ikscene->scene->setParam(iTaSC::Scene::MAX_TIMESTEP, 1.0); + ikscene->solver->setParam(iTaSC::Solver::DLS_QMAX, 0.52); + ikscene->armature->setControlParameter( + CONSTRAINT_ID_ALL, iTaSC::Armature::ID_JOINT, iTaSC::ACT_FEEDBACK, 0.8); + } + } + } } void itasc_test_constraint(struct Object *ob, struct bConstraint *cons) { - struct bKinematicConstraint *data = (struct bKinematicConstraint *)cons->data; - - /* only for IK constraint */ - if (cons->type != CONSTRAINT_TYPE_KINEMATIC || data == NULL) - return; - - switch (data->type) { - case CONSTRAINT_IK_COPYPOSE: - case CONSTRAINT_IK_DISTANCE: - /* cartesian space constraint */ - break; - } + struct bKinematicConstraint *data = (struct bKinematicConstraint *)cons->data; + + /* only for IK constraint */ + if (cons->type != CONSTRAINT_TYPE_KINEMATIC || data == NULL) + return; + + switch (data->type) { + case CONSTRAINT_IK_COPYPOSE: + case CONSTRAINT_IK_DISTANCE: + /* cartesian space constraint */ + break; + } } diff --git a/source/blender/ikplugin/intern/itasc_plugin.h b/source/blender/ikplugin/intern/itasc_plugin.h index 9e9adf74241..e7a319809b7 100644 --- a/source/blender/ikplugin/intern/itasc_plugin.h +++ b/source/blender/ikplugin/intern/itasc_plugin.h @@ -22,7 +22,6 @@ * \ingroup ikplugin */ - #ifndef __ITASC_PLUGIN_H__ #define __ITASC_PLUGIN_H__ @@ -32,9 +31,16 @@ extern "C" { #endif -void itasc_initialize_tree(struct Depsgraph *depsgraph, struct Scene *scene, struct Object *ob, float ctime); -void itasc_execute_tree(struct Depsgraph *depsgraph, struct Scene *scene, struct Object *ob, struct bPoseChannel *pchan_root, float ctime); -void itasc_release_tree(struct Scene *scene, struct Object *ob, float ctime); +void itasc_initialize_tree(struct Depsgraph *depsgraph, + struct Scene *scene, + struct Object *ob, + float ctime); +void itasc_execute_tree(struct Depsgraph *depsgraph, + struct Scene *scene, + struct Object *ob, + struct bPoseChannel *pchan_root, + float ctime); +void itasc_release_tree(struct Scene *scene, struct Object *ob, float ctime); void itasc_clear_data(struct bPose *pose); void itasc_clear_cache(struct bPose *pose); void itasc_update_param(struct bPose *pose); @@ -44,4 +50,4 @@ void itasc_test_constraint(struct Object *ob, struct bConstraint *cons); } #endif -#endif /* __ITASC_PLUGIN_H__ */ +#endif /* __ITASC_PLUGIN_H__ */ |