From 1597ad9377460453845b48af69ea888e32297cc1 Mon Sep 17 00:00:00 2001 From: Campbell Barton Date: Sun, 1 Jul 2012 09:54:44 +0000 Subject: style cleanup --- source/blender/ikplugin/intern/iksolver_plugin.c | 131 ++- source/blender/ikplugin/intern/itasc_plugin.cpp | 1053 +++++++++++----------- 2 files changed, 590 insertions(+), 594 deletions(-) (limited to 'source/blender/ikplugin') diff --git a/source/blender/ikplugin/intern/iksolver_plugin.c b/source/blender/ikplugin/intern/iksolver_plugin.c index b9980b1401d..d544e6501fc 100644 --- a/source/blender/ikplugin/intern/iksolver_plugin.c +++ b/source/blender/ikplugin/intern/iksolver_plugin.c @@ -21,7 +21,7 @@ * The Original Code is: all of this file. * * Original author: Benoit Bolsee - * Contributor(s): + * Contributor(s): * * ***** END GPL LICENSE BLOCK ***** */ @@ -30,7 +30,6 @@ * \ingroup ikplugin */ - #include "MEM_guardedalloc.h" #include "BIK_api.h" @@ -75,25 +74,25 @@ static void initialize_posetree(struct Object *UNUSED(ob), bPoseChannel *pchan_t } } 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) { @@ -116,11 +115,11 @@ static void initialize_posetree(struct Object *UNUSED(ob), bPoseChannel *pchan_t 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++) { @@ -128,7 +127,7 @@ static void initialize_posetree(struct Object *UNUSED(ob), bPoseChannel *pchan_t tree->parent[a] = a - 1; } target->tip = segcount - 1; - + /* AND! link the tree to the root */ BLI_addtail(&pchan_root->iktree, tree); } @@ -154,7 +153,7 @@ static void initialize_posetree(struct Object *UNUSED(ob), bPoseChannel *pchan_t 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; @@ -177,7 +176,7 @@ static void initialize_posetree(struct Object *UNUSED(ob), bPoseChannel *pchan_t tree->parent[tree->totchannel + a] = tree->totchannel + a - 1; } tree->parent[tree->totchannel] = parent; - + tree->totchannel = newsize; } @@ -210,12 +209,12 @@ static void make_dmats(bPoseChannel *pchan) static void where_is_ik_bone(bPoseChannel *pchan, float ik_mat[][3]) // nr = to detect if this is first bone { float vec[3], ikmat[4][4]; - + copy_m4_m3(ikmat, ik_mat); - + if (pchan->parent) mul_serie_m4(pchan->pose_mat, pchan->parent->pose_mat, pchan->chan_mat, ikmat, NULL, NULL, NULL, NULL, NULL); - else + else mult_m4_m4m4(pchan->pose_mat, pchan->chan_mat, ikmat); /* calculate head */ @@ -253,13 +252,13 @@ static void execute_posetree(struct Scene *scene, Object *ob, PoseTree *tree) if (tree->totchannel == 0) return; - + iktree = MEM_mallocN(sizeof(void *) * tree->totchannel, "ik tree"); for (a = 0; a < tree->totchannel; a++) { 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)) @@ -268,72 +267,72 @@ static void execute_posetree(struct Scene *scene, Object *ob, PoseTree *tree) 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]); - + /* compute rest basis and its inverse */ copy_m3_m3(rest_basis, bone->bone_mat); copy_m3_m3(irest_basis, bone->bone_mat); transpose_m3(irest_basis); - + /* 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); - + /* basis must be pure rotation */ normalize_m3(basis); - + /* transform offset into local bone space */ normalize_m3(iR_parmat); 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)) { float ikstretch = pchan->ikstretch * pchan->ikstretch; IK_SetStiffness(seg, IK_TRANS_Y, MIN2(1.0f - ikstretch, 0.99f)); @@ -359,31 +358,31 @@ static void execute_posetree(struct Scene *scene, Object *ob, PoseTree *tree) else unit_m4(rootmat); copy_v3_v3(rootmat[3], pchan->pose_head); - + mult_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) */ get_constraint_target_matrix(scene, target->con, 0, CONSTRAINT_OBTYPE_OBJECT, ob, rootmat, 1.0); - + /* and set and transform goal */ mult_m4_m4m4(goal, goalinv, rootmat); - + copy_v3_v3(goalpos, goal[3]); copy_m3_m4(goalrot, goal); - + /* same for pole vector target */ if (data->poletar) { get_constraint_target_matrix(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; @@ -398,7 +397,7 @@ static void execute_posetree(struct Scene *scene, Object *ob, PoseTree *tree) * a smooth transition */ resultblend = 1; resultinf = target->con->enforce; - + if (data->flag & CONSTRAINT_IK_GETANGLE) { poleangledata = data; data->flag &= ~CONSTRAINT_IK_GETANGLE; @@ -411,28 +410,28 @@ static void execute_posetree(struct Scene *scene, Object *ob, PoseTree *tree) 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_serie_m4(world_pose, goalinv, ob->obmat, end_pose, NULL, NULL, NULL, NULL, NULL); - + /* 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->weight != 0.0f) { if (poleconstrain) IK_SolverSetPoleVectorConstraint(solver, iktarget, goalpos, @@ -457,30 +456,30 @@ static void execute_posetree(struct Scene *scene, Object *ob, PoseTree *tree) 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); @@ -491,10 +490,10 @@ static void execute_posetree(struct Scene *scene, Object *ob, PoseTree *tree) 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); } @@ -514,7 +513,7 @@ static void free_posetree(PoseTree *tree) void iksolver_initialize_tree(struct Scene *UNUSED(scene), struct Object *ob, float UNUSED(ctime)) { 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! @@ -527,11 +526,11 @@ void iksolver_execute_tree(struct Scene *scene, struct Object *ob, struct bPose while (pchan->iktree.first) { PoseTree *tree = pchan->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 @@ -541,18 +540,18 @@ void iksolver_execute_tree(struct Scene *scene, struct Object *ob, struct bPose } /* 5. execute the IK solver */ execute_posetree(scene, ob, tree); - - /* 6. apply the differences to the channels, + + /* 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->iktree, tree); free_posetree(tree); diff --git a/source/blender/ikplugin/intern/itasc_plugin.cpp b/source/blender/ikplugin/intern/itasc_plugin.cpp index 048dd955726..93934b41f28 100644 --- a/source/blender/ikplugin/intern/itasc_plugin.cpp +++ b/source/blender/ikplugin/intern/itasc_plugin.cpp @@ -21,7 +21,7 @@ * The Original Code is: all of this file. * * Original author: Benoit Bolsee - * Contributor(s): + * Contributor(s): * * ***** END GPL LICENSE BLOCK ***** */ @@ -30,7 +30,6 @@ * \ingroup ikplugin */ - #include #include #include @@ -73,23 +72,22 @@ extern "C" { bItasc DefIKParam; // in case of animation mode, feedback and timestep is fixed -#define ANIM_TIMESTEP 1.0 -#define ANIM_FEEDBACK 0.8 -#define ANIM_QMAX 0.52 +#define ANIM_TIMESTEP 1.0 +#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_Data { + 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 @@ -160,8 +158,7 @@ struct IK_Channel { } }; -struct IK_Scene -{ +struct IK_Scene { struct Scene *blscene; IK_Scene* next; int numchan; // number of channel in pchan @@ -201,11 +198,11 @@ struct IK_Scene // delete scene first if (scene) delete scene; - for (std::vector::iterator it = targets.begin(); it != targets.end(); ++it) + for (std::vector::iterator it = targets.begin(); it != targets.end(); ++it) delete (*it); targets.clear(); if (channels) - delete [] channels; + delete[] channels; if (solver) delete solver; if (armature) @@ -239,38 +236,38 @@ enum IK_SegmentAxis { static int initialize_chain(Object *ob, bPoseChannel *pchan_tip, bConstraint *con) { - bPoseChannel *curchan, *pchan_root=NULL, *chanlist[256], **oldchan; + 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; + int a, t, segcount = 0, size, newsize, *oldparent, parent, rootbone, treecount; + + data = (bKinematicConstraint *)con->data; - data=(bKinematicConstraint*)con->data; - /* exclude tip from chain? */ if (!(data->flag & CONSTRAINT_IK_TIP)) - pchan_tip= pchan_tip->parent; - + 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) { + for (curchan = pchan_tip; curchan; curchan = curchan->parent) { pchan_root = curchan; - - if (++segcount > 255) // 255 is weak + + if (++segcount > 255) // 255 is weak break; - if (segcount==rootbone) { - // reached this end of the chain but if the chain is overlapping with a + 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) && curchan->iktree.first == NULL) { rootbone++; continue; } - break; + break; } if (curchan->iktree.first != NULL) - // Oh oh, there is already a chain starting from this channel and our chain is longer... + // 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; @@ -280,86 +277,86 @@ static int initialize_chain(Object *ob, bPoseChannel *pchan_tip, bConstraint *co if ((pchan_root->flag & POSE_CHAIN) && pchan_root->iktree.first == NULL) 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; + 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; + 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; + tree = (PoseTree *)pchan_root->iktree.first; - if (tree==NULL) { + if (tree == NULL) { /* make new tree */ - tree= (PoseTree*)MEM_callocN(sizeof(PoseTree), "posetree"); + tree = (PoseTree *)MEM_callocN(sizeof(PoseTree), "posetree"); - tree->iterations= data->iterations; - tree->totchannel= segcount; + 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; apchan[a]= chanlist[segcount-a-1]; - tree->parent[a]= a-1; + + 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; - + 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); + 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); + size = MIN2(segcount, tree->totchannel); a = t = 0; - while (atotchannel) { + while (a < size && t < tree->totchannel) { // locate first matching channel - for (;ttotchannel && tree->pchan[t]!=chanlist[segcount-a-1];t++); - if (t>=tree->totchannel) + for (; t < tree->totchannel && tree->pchan[t] != chanlist[segcount - a - 1]; t++) ; + if (t >= tree->totchannel) break; - for (; atotchannel && tree->pchan[t]==chanlist[segcount-a-1]; a++, t++); + for (; a < size && t < tree->totchannel && tree->pchan[t] == chanlist[segcount - a - 1]; a++, t++) ; } - segcount= segcount-a; - target->tip= tree->totchannel + segcount - 1; + 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) + 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); + 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; apchan[tree->totchannel+a]= chanlist[segcount-a-1]; - tree->parent[tree->totchannel+a]= tree->totchannel+a-1; + 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; + tree->parent[tree->totchannel] = parent; + + tree->totchannel = newsize; } // reusing tree treecount = 0; @@ -381,7 +378,7 @@ static bool is_cartesian_constraint(bConstraint *con) static bool constraint_valid(bConstraint *con) { - bKinematicConstraint* data=(bKinematicConstraint*)con->data; + bKinematicConstraint *data = (bKinematicConstraint *)con->data; if (data->flag & CONSTRAINT_IK_AUTO) return true; @@ -389,9 +386,9 @@ static bool constraint_valid(bConstraint *con) return false; if (is_cartesian_constraint(con)) { /* cartesian space constraint */ - if (data->tar==NULL) + if (data->tar == NULL) return false; - if (data->tar->type==OB_ARMATURE && data->subtarget[0]==0) + if (data->tar->type == OB_ARMATURE && data->subtarget[0] == 0) return false; } return true; @@ -404,8 +401,8 @@ int initialize_scene(Object *ob, bPoseChannel *pchan_tip) /* 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) { + 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); } @@ -413,23 +410,23 @@ int initialize_scene(Object *ob, bPoseChannel *pchan_tip) return treecount; } -static IK_Data* get_ikdata(bPose *pose) +static IK_Data *get_ikdata(bPose *pose) { if (pose->ikdata) - return (IK_Data*)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; + return (IK_Data *)pose->ikdata; } 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)); + double t = KDL::sqrt(R(0, 0) * R(0, 0) + R(0, 1) * R(0, 1)); - if (t > 16.0*KDL::epsilon) { + 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)); @@ -447,7 +444,7 @@ static double ComputeTwist(const KDL::Rotation& 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; } @@ -457,102 +454,102 @@ 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; + 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; + 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 ) { + 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)))); + 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 ) { + 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)))); + 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; + 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) { - IK_Target* target = (IK_Target*)param; + IK_Target *target = (IK_Target *)param; // compute next target position // get target matrix from constraint. - bConstraint* constraint = (bConstraint*)target->blenderConstraint; + bConstraint *constraint = (bConstraint *)target->blenderConstraint; float tarmat[4][4]; get_constraint_target_matrix(target->blscene, constraint, 0, CONSTRAINT_OBTYPE_OBJECT, target->owner, tarmat, 1.0); @@ -564,14 +561,14 @@ static bool target_callback(const iTaSC::Timestamp& timestamp, const iTaSC::Fram // 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; + 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_serie_m4(restmat, target->owner->obmat, chanmat, target->eeRest, NULL, NULL, NULL, NULL, NULL); - } + } else { mult_m4_m4m4(restmat, target->owner->obmat, target->eeRest); } @@ -584,13 +581,13 @@ static bool target_callback(const iTaSC::Timestamp& timestamp, const iTaSC::Fram static bool base_callback(const iTaSC::Timestamp& timestamp, const iTaSC::Frame& current, iTaSC::Frame& next, void *param) { - IK_Scene* ikscene = (IK_Scene*)param; + IK_Scene *ikscene = (IK_Scene *)param; // compute next armature base pose - // algorithm: + // 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; + bPoseChannel *pchan = ikscene->channels[0].pchan; float rootmat[4][4]; if (pchan->parent) { pchan = pchan->parent; @@ -602,7 +599,7 @@ static bool base_callback(const iTaSC::Timestamp& timestamp, const iTaSC::Frame& // iTaSC armature is scaled to object scale, scale the base frame too ikscene->baseFrame.p *= ikscene->blScale; mult_m4_m4m4(rootmat, ikscene->blArmature->obmat, chanmat); - } + } else { copy_m4_m4(rootmat, ikscene->blArmature->obmat); ikscene->baseFrame = iTaSC::F_identity; @@ -611,11 +608,11 @@ static bool base_callback(const iTaSC::Timestamp& timestamp, const iTaSC::Frame& // 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; + 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 @@ -642,48 +639,48 @@ static bool base_callback(const iTaSC::Timestamp& timestamp, const iTaSC::Frame& 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; + 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(); + KDL::Vector rootpos = rootframe.p - length *rootframe.M.UnitY(); - // compute main directions + // compute main directions KDL::Vector dir = KDL::Normalize(endpos - rootpos); - KDL::Vector poledir = KDL::Normalize(goalpos-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); + 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); + KDL::Vector x = KDL::Normalize(dir * up); endrot.UnitX(x); - endrot.UnitY(KDL::Normalize(x*dir)); + endrot.UnitY(KDL::Normalize(x * dir)); endrot.UnitZ(-dir); - // for the polar target - x = KDL::Normalize(poledir*poleup); + // for the polar target + x = KDL::Normalize(poledir * poleup); polerot.UnitX(x); - polerot.UnitY(KDL::Normalize(x*poledir)); + 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()); + 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; + 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& timestamp, iTaSC::ConstraintValues *const _values, unsigned int _nvalues, void *_param) { - IK_Target* iktarget =(IK_Target*)_param; + IK_Target *iktarget = (IK_Target *)_param; bKinematicConstraint *condata = (bKinematicConstraint *)iktarget->blenderConstraint->data; - iTaSC::ConstraintValues* values = _values; - bItasc* ikparam = (bItasc*) iktarget->owner->pose->ikparam; + iTaSC::ConstraintValues *values = _values; + bItasc *ikparam = (bItasc *) iktarget->owner->pose->ikparam; // we need default parameters - if (!ikparam) + if (!ikparam) ikparam = &DefIKParam; if (iktarget->blenderConstraint->flag & CONSTRAINT_OFF) { @@ -702,14 +699,14 @@ static bool copypose_callback(const iTaSC::Timestamp& timestamp, iTaSC::Constrai if (iktarget->controlType & iTaSC::CopyPose::CTL_POSITION) { // update error values->alpha = condata->weight; - values->action = iTaSC::ACT_ALPHA|iTaSC::ACT_FEEDBACK; + 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->action = iTaSC::ACT_ALPHA | iTaSC::ACT_FEEDBACK; values->feedback = (iktarget->simulation) ? ikparam->feedback : ANIM_FEEDBACK; values++; } @@ -717,36 +714,36 @@ static bool copypose_callback(const iTaSC::Timestamp& timestamp, iTaSC::Constrai 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; + 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; inumber; ++i, ++value) + 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; inumber; ++i, ++value) + 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& timestamp, iTaSC::ConstraintValues *const _values, unsigned int _nvalues, void *_param) { - IK_Target* iktarget =(IK_Target*)_param; + IK_Target *iktarget = (IK_Target *)_param; bKinematicConstraint *condata = (bKinematicConstraint *)iktarget->blenderConstraint->data; - iTaSC::ConstraintValues* values = _values; - bItasc* ikparam = (bItasc*) iktarget->owner->pose->ikparam; + iTaSC::ConstraintValues *values = _values; + bItasc *ikparam = (bItasc *) iktarget->owner->pose->ikparam; // we need default parameters - if (!ikparam) + if (!ikparam) ikparam = &DefIKParam; // update weight according to mode @@ -755,30 +752,30 @@ static bool distance_callback(const iTaSC::Timestamp& timestamp, iTaSC::Constrai } 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; + values->alpha = (values->values[0].y > condata->dist) ? condata->weight : 0.0; break; case LIMITDIST_OUTSIDE: - values->values[0].yd = condata->dist*1.05; + values->alpha = (values->values[0].y < condata->dist) ? condata->weight : 0.0; break; default: - values->values[0].yd = condata->dist; + 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->values[0].action = iTaSC::ACT_VALUE | iTaSC::ACT_FEEDBACK; values->feedback = (iktarget->simulation) ? ikparam->feedback : ANIM_FEEDBACK; } } @@ -786,16 +783,16 @@ static bool distance_callback(const iTaSC::Timestamp& timestamp, iTaSC::Constrai 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); } -static bool joint_callback(const iTaSC::Timestamp& timestamp, iTaSC::ConstraintValues* const _values, unsigned int _nvalues, void* _param) +static bool joint_callback(const iTaSC::Timestamp& timestamp, 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; + 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 @@ -808,11 +805,11 @@ static bool joint_callback(const iTaSC::Timestamp& timestamp, iTaSC::ConstraintV 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); + 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]); + axis_angle_to_mat3(rmat, &chan->quat[1], chan->quat[0]); } else { /* quats are normalised before use to eliminate scaling issues */ @@ -820,45 +817,45 @@ static bool joint_callback(const iTaSC::Timestamp& timestamp, iTaSC::ConstraintV 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]); + 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; + 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++) { + 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; @@ -875,30 +872,30 @@ static int convert_channels(IK_Scene *ikscene, PoseTree *tree) int a, flag, njoint; njoint = 0; - for (a=0, ikchan = ikscene->channels; anumchan; ++a, ++ikchan) { - pchan= tree->pchan[a]; + 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->parent = (a > 0) ? tree->parent[a] : -1; ikchan->owner = ikscene->blArmature; - + /* 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)) + (!(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)) + (!(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)) + (!(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; } @@ -929,49 +926,49 @@ static int convert_channels(IK_Scene *ikscene, PoseTree *tree) * 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; + 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; @@ -992,8 +989,8 @@ static void convert_pose(IK_Scene *ikscene) 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 rmat[4][4]; // rest pose of bone with parent taken into account + float bmat[4][4]; // difference float scale; double *rot; int a, joint; @@ -1001,10 +998,10 @@ static void convert_pose(IK_Scene *ikscene) // 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; anumchan && jointnumjoint; ++a, ++ikchan) { - pchan= ikchan->pchan; - bone= pchan->bone; - + 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); @@ -1018,9 +1015,9 @@ static void convert_pose(IK_Scene *ikscene) 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; - } + // compute actual length + rot[ikchan->ndof - 1] = len_v3v3(pchan->pose_tail, pchan->pose_head) * scale; + } rot += ikchan->ndof; joint += ikchan->ndof; } @@ -1038,32 +1035,32 @@ static void BKE_pose_rest(IK_Scene *ikscene) // assume uniform scaling and take Y scale as general scale for the armature scale = len_v3(ikscene->blArmature->obmat[1]); - // rest pose is 0 + // rest pose is 0 SetToZero(ikscene->jointArray); // except for transY joints rot = ikscene->jointArray(0); - for (joint=a=0, ikchan = ikscene->channels; anumchan && jointnumjoint; ++a, ++ikchan) { - pchan= ikchan->pchan; - bone= pchan->bone; + 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 - 1] = bone->length * scale; rot += ikchan->ndof; joint += ikchan->ndof; } } -static IK_Scene* convert_tree(Scene *blscene, Object *ob, bPoseChannel *pchan) +static IK_Scene *convert_tree(Scene *blscene, Object *ob, bPoseChannel *pchan) { - PoseTree *tree = (PoseTree*)pchan->iktree.first; + 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; + iTaSC::Armature *arm; + iTaSC::Scene *scene; + IK_Scene *ikscene; + IK_Channel *ikchan; KDL::Frame initPose; KDL::Rotation boneRot; Bone *bone; @@ -1084,7 +1081,7 @@ static IK_Scene* convert_tree(Scene *blscene, Object *ob, bPoseChannel *pchan) ikscene->numchan = tree->totchannel; ikscene->armature = arm; ikscene->scene = scene; - ikparam = (bItasc*)ob->pose->ikparam; + ikparam = (bItasc *)ob->pose->ikparam; ingame = (ob->pose->flag & POSE_GAME_ENGINE); if (!ikparam) { // you must have our own copy @@ -1096,7 +1093,7 @@ static IK_Scene* convert_tree(Scene *blscene, Object *ob, bPoseChannel *pchan) // shorter than in animation => move to auto step automatically and set // the target substep duration via min/max if (!(ikparam->flag & ITASC_AUTO_STEP)) { - float timestep = blscene->r.frs_sec_base/blscene->r.frs_sec; + float timestep = blscene->r.frs_sec_base / blscene->r.frs_sec; if (ikparam->numstep > 0) timestep /= ikparam->numstep; // with equal min and max, the algorythm will take this step and the indicative substep most of the time @@ -1109,24 +1106,24 @@ static IK_Scene* convert_tree(Scene *blscene, Object *ob, bPoseChannel *pchan) 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; + 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; + ikscene->blInvScale = (ikscene->blScale < KDL::epsilon) ? 0.0f : 1.0f / ikscene->blScale; - std::string joint; - std::string root("root"); - std::string parent; + std::string joint; + std::string root("root"); + std::string parent; std::vector weights; double weight[3]; // build the array of joints corresponding to the IK chain @@ -1141,22 +1138,22 @@ static IK_Scene* convert_tree(Scene *blscene, Object *ob, bPoseChannel *pchan) BKE_pose_rest(ikscene); } rot = ikscene->jointArray(0); - for (a=0, ikchan = ikscene->channels; atotchannel; ++a, ++ikchan) { - pchan= ikchan->pchan; - bone= pchan->bone; + for (a = 0, ikchan = ikscene->channels; a < tree->totchannel; ++a, ++ikchan) { + pchan = ikchan->pchan; + bone = pchan->bone; KDL::Frame tip(iTaSC::F_identity); 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]); + 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]); KDL::Vector bpos(bone->head[0], bone->head[1], bone->head[2]); bpos *= ikscene->blScale; KDL::Frame head(brot, bpos); - + // rest pose length of the bone taking scaling into account - length= bone->length*ikscene->blScale; + length = bone->length * ikscene->blScale; parent = (a > 0) ? ikscene->channels[tree->parent[a]].tail : root; // first the fixed segment to the bone head if (head.p.Norm() > KDL::epsilon || head.M.GetRot().Norm() > KDL::epsilon) { @@ -1170,95 +1167,95 @@ static IK_Scene* convert_tree(Scene *blscene, Object *ob, bPoseChannel *pchan) 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]); + 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 - if (!(ikchan->jointType & IK_TRANSY)) { - 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; + case 0: + // fixed bone + if (!(ikchan->jointType & IK_TRANSY)) { + 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[1], tip); + ret = arm->addSegment(joint, parent, KDL::Joint::RotY, rot[0], 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); + 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]); - } - 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; + 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]); - float ikstretch = pchan->ikstretch*pchan->ikstretch; - weight[1] = (1.0-MIN2(1.0-ikstretch, 0.99)); + ret = arm->addSegment(joint, parent, KDL::Joint::TransY, rot[ikchan->ndof - 1]); + float ikstretch = pchan->ikstretch * pchan->ikstretch; + weight[1] = (1.0 - MIN2(1.0 - ikstretch, 0.99)); weights.push_back(weight[1]); } if (!ret) @@ -1269,7 +1266,7 @@ static IK_Scene* convert_tree(Scene *blscene, Object *ob, bPoseChannel *pchan) ikchan->head = parent; // in case of error ret = false; - if ((ikchan->jointType & IK_XDOF) && (pchan->ikflag & (BONE_IK_XLIMIT|BONE_IK_ROTCTL))) { + if ((ikchan->jointType & IK_XDOF) && (pchan->ikflag & (BONE_IK_XLIMIT | BONE_IK_ROTCTL))) { joint = bone->name; joint += ":RX"; if (pchan->ikflag & BONE_IK_XLIMIT) { @@ -1281,7 +1278,7 @@ static IK_Scene* convert_tree(Scene *blscene, Object *ob, bPoseChannel *pchan) break; } } - if ((ikchan->jointType & IK_YDOF) && (pchan->ikflag & (BONE_IK_YLIMIT|BONE_IK_ROTCTL))) { + if ((ikchan->jointType & IK_YDOF) && (pchan->ikflag & (BONE_IK_YLIMIT | BONE_IK_ROTCTL))) { joint = bone->name; joint += ":RY"; if (pchan->ikflag & BONE_IK_YLIMIT) { @@ -1293,7 +1290,7 @@ static IK_Scene* convert_tree(Scene *blscene, Object *ob, bPoseChannel *pchan) break; } } - if ((ikchan->jointType & IK_ZDOF) && (pchan->ikflag & (BONE_IK_ZLIMIT|BONE_IK_ROTCTL))) { + if ((ikchan->jointType & IK_ZDOF) && (pchan->ikflag & (BONE_IK_ZLIMIT | BONE_IK_ROTCTL))) { joint = bone->name; joint += ":RZ"; if (pchan->ikflag & BONE_IK_ZLIMIT) { @@ -1305,7 +1302,7 @@ static IK_Scene* convert_tree(Scene *blscene, Object *ob, bPoseChannel *pchan) break; } } - if ((ikchan->jointType & IK_SWING) && (pchan->ikflag & (BONE_IK_XLIMIT|BONE_IK_ZLIMIT|BONE_IK_ROTCTL))) { + 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) { @@ -1336,13 +1333,13 @@ static IK_Scene* convert_tree(Scene *blscene, Object *ob, bPoseChannel *pchan) 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; + 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(); + IK_Target *iktarget = new IK_Target(); ikscene->targets.push_back(iktarget); iktarget->ee = arm->addEndEffector(ikscene->channels[target->tip].tail); if (iktarget->ee == -1) { @@ -1372,7 +1369,7 @@ static IK_Scene* convert_tree(Scene *blscene, Object *ob, bPoseChannel *pchan) ikscene->polarConstraint = polarcon; } // we can now add the armature - // the armature is based on a moving frame. + // 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); @@ -1392,8 +1389,8 @@ static IK_Scene* convert_tree(Scene *blscene, Object *ob, bPoseChannel *pchan) // set the weight e_matrix& Wq = arm->getWq(); assert(Wq.cols() == (int)weights.size()); - for (int q=0; qpchan[0] is the root channel of the tree @@ -1401,9 +1398,9 @@ static IK_Scene* convert_tree(Scene *blscene, Object *ob, bPoseChannel *pchan) float invBaseFrame[4][4]; pchan = ikscene->channels[0].pchan; if (pchan->parent) { - // it has a parent, get the pose matrix from it + // it has a parent, get the pose matrix from it float baseFrame[4][4]; - pchan = pchan->parent; + 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); @@ -1413,10 +1410,10 @@ static IK_Scene* convert_tree(Scene *blscene, Object *ob, bPoseChannel *pchan) unit_m4(invBaseFrame); } // finally add the constraint - for (t=0; ttargets.size(); t++) { - IK_Target* iktarget = ikscene->targets[t]; + for (t = 0; t < ikscene->targets.size(); t++) { + IK_Target *iktarget = ikscene->targets[t]; iktarget->blscene = blscene; - condata= (bKinematicConstraint*)iktarget->blenderConstraint->data; + condata = (bKinematicConstraint *)iktarget->blenderConstraint->data; pchan = tree->pchan[iktarget->channel]; unsigned int controltype, bonecnt; double bonelen; @@ -1424,16 +1421,16 @@ static IK_Scene* convert_tree(Scene *blscene, Object *ob, bPoseChannel *pchan) // 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; + 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 mult_m4_m4m4(iktarget->eeRest, invBaseFrame, mat); - iktarget->eeBlend = (!ikscene->polarConstraint && condata->type==CONSTRAINT_IK_COPYPOSE) ? true : false; + 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); @@ -1443,59 +1440,59 @@ static IK_Scene* convert_tree(Scene *blscene, Object *ob, bPoseChannel *pchan) 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; + 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 - 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; + 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()) { + !scene->addCache(ikscene->cache) || + !scene->addSolver(ikscene->solver) || + !scene->initialize()) { delete ikscene; ikscene = NULL; } @@ -1507,13 +1504,13 @@ static void create_scene(Scene *scene, Object *ob) bPoseChannel *pchan; // create the IK scene - for (pchan= (bPoseChannel *)ob->pose->chanbase.first; pchan; pchan= (bPoseChannel *)pchan->next) { + 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; + PoseTree *tree = (PoseTree *)pchan->iktree.first; if (tree) { - IK_Data* ikdata = get_ikdata(ob->pose); + IK_Data *ikdata = get_ikdata(ob->pose); // convert tree in iTaSC::Scene - IK_Scene* ikscene = convert_tree(scene, ob, pchan); + IK_Scene *ikscene = convert_tree(scene, ob, pchan); if (ikscene) { ikscene->next = ikdata->first; ikdata->first = ikscene; @@ -1526,7 +1523,7 @@ static void create_scene(Scene *scene, Object *ob) if (tree->parent) MEM_freeN(tree->parent); if (tree->basis_change) MEM_freeN(tree->basis_change); MEM_freeN(tree); - tree = (PoseTree*)pchan->iktree.first; + tree = (PoseTree *)pchan->iktree.first; } } } @@ -1537,12 +1534,12 @@ static int init_scene(Object *ob) { // check also if scaling has changed float scale = len_v3(ob->obmat[1]); - IK_Scene* scene; + IK_Scene *scene; if (ob->pose->ikdata) { - for (scene = ((IK_Data*)ob->pose->ikdata)->first; - scene != NULL; - scene = scene->next) { + 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; @@ -1551,31 +1548,31 @@ static int init_scene(Object *ob) return 0; } -static void execute_scene(Scene* blscene, IK_Scene* ikscene, bItasc* ikparam, float ctime, float frtime) +static void execute_scene(Scene *blscene, IK_Scene *ikscene, bItasc *ikparam, float ctime, float frtime) { int i; - IK_Channel* ikchan; + IK_Channel *ikchan; if (ikparam->flag & ITASC_SIMULATION) { - for (i=0, ikchan=ikscene->channels; inumchan; i++, ++ikchan) { + 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->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; inumchan; i++, ++ikchan) { + for (i = 0, ikchan = ikscene->channels; i < ikscene->numchan; i++, ++ikchan) { if (!(ikchan->pchan->flag & POSE_DONE)) BKE_pose_where_is_bone(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->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]; + for (i = ikscene->targets.size(); i > 0; --i) { + IK_Target *iktarget = ikscene->targets[i - 1]; if (!(iktarget->blenderConstraint->flag & CONSTRAINT_OFF)) break; } @@ -1592,7 +1589,7 @@ static void execute_scene(Scene* blscene, IK_Scene* ikscene, bItasc* ikparam, fl 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); @@ -1601,15 +1598,15 @@ static void execute_scene(Scene* blscene, IK_Scene* ikscene, bItasc* ikparam, fl 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 + // 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); + 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) @@ -1618,14 +1615,14 @@ static void execute_scene(Scene* blscene, IK_Scene* ikscene, bItasc* ikparam, fl else { // can take the cache as a start point. sts -= cts; - timestep = sts/1000.0; + timestep = sts / 1000.0; } } // don't cache if we are reiterating because we don't want to distroy the cache unnecessarily ikscene->scene->update(timestamp, timestep, numstep, false, !reiterate, simulation); if (reiterate) { // how many times do we reiterate? - for (i = 0; inumiter; i++) { + for (i = 0; i < ikparam->numiter; i++) { if (ikscene->armature->getMaxJointChange() < ikparam->precision || ikscene->armature->getMaxEndEffectorChange() < ikparam->precision) { @@ -1639,11 +1636,11 @@ static void execute_scene(Scene* blscene, IK_Scene* ikscene, bItasc* ikparam, fl } } // compute constraint error - for (i=ikscene->targets.size(); i > 0; --i) { - IK_Target* iktarget = ikscene->targets[i-1]; + for (i = ikscene->targets.size(); i > 0; --i) { + IK_Target *iktarget = ikscene->targets[i - 1]; if (!(iktarget->blenderConstraint->flag & CONSTRAINT_OFF)) { unsigned int nvalues; - const iTaSC::ConstraintValues* values; + const iTaSC::ConstraintValues *values; values = iktarget->constraint->getControlParameters(&nvalues); iktarget->errorCallback(values, nvalues, iktarget); } @@ -1654,22 +1651,22 @@ static void execute_scene(Scene* blscene, IK_Scene* ikscene, bItasc* ikparam, fl // 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; + iTaSC::Armature *arm = ikscene->armature; KDL::Frame frame; double q_rest[3], q[3]; - const KDL::Joint* joint; - const KDL::Frame* tip; - bPoseChannel* pchan; + const KDL::Joint *joint; + const KDL::Frame *tip; + bPoseChannel *pchan; float scale; float length; float yaxis[3]; - for (i=0, ikchan=ikscene->channels; inumchan; ++i, ++ikchan) { + 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; @@ -1682,10 +1679,10 @@ static void execute_scene(Scene* blscene, IK_Scene* ikscene, bItasc* ikparam, fl break; if (joint->getType() == KDL::Joint::TransY) { // stretch bones have a TY joint, compute the scale - scale = (float)(q[0]/q_rest[0]); + 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) @@ -1710,7 +1707,7 @@ static void execute_scene(Scene* blscene, IK_Scene* ikscene, bItasc* ikparam, fl mul_v3_fl(pchan->pose_mat[1], scale); mul_v3_fl(pchan->pose_mat[2], scale); } - if (inumchan) { + if (i < ikscene->numchan) { // big problem ; } @@ -1732,11 +1729,11 @@ void itasc_initialize_tree(struct Scene *scene, Object *ob, float ctime) 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) { + 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 + // if at least one tree, create the scenes from the PoseTree stored in the channels if (count) create_scene(scene, ob); itasc_update_param(ob->pose); @@ -1747,14 +1744,14 @@ void itasc_initialize_tree(struct Scene *scene, Object *ob, float ctime) void itasc_execute_tree(struct Scene *scene, struct Object *ob, struct bPoseChannel *pchan, float ctime) { if (ob->pose->ikdata) { - IK_Data* ikdata = (IK_Data*)ob->pose->ikdata; - bItasc* ikparam = (bItasc*) ob->pose->ikparam; + 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) { + for (IK_Scene *ikscene = ikdata->first; ikscene; ikscene = ikscene->next) { if (ikscene->channels[0].pchan == pchan) { - float timestep = scene->r.frs_sec_base/scene->r.frs_sec; + float timestep = scene->r.frs_sec_base / scene->r.frs_sec; if (ob->pose->flag & POSE_GAME_ENGINE) { timestep = ob->pose->ctime; // limit the timestep to avoid excessive number of iteration @@ -1776,8 +1773,8 @@ void itasc_release_tree(struct Scene *scene, struct Object *ob, float ctime) 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) { + IK_Data *ikdata = (IK_Data *)pose->ikdata; + for (IK_Scene *scene = ikdata->first; scene; scene = ikdata->first) { ikdata->first = scene->next; delete scene; } @@ -1789,8 +1786,8 @@ void itasc_clear_data(struct bPose *pose) 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) { + 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); @@ -1801,12 +1798,12 @@ void itasc_clear_cache(struct bPose *pose) 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) { + 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); + 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); @@ -1814,7 +1811,7 @@ void itasc_update_param(struct bPose *pose) ikscene->armature->setControlParameter(CONSTRAINT_ID_ALL, iTaSC::Armature::ID_JOINT, iTaSC::ACT_FEEDBACK, ikparam->feedback); } else { - // in animation mode timestep is 1s by convention => + // 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); @@ -1834,10 +1831,10 @@ void itasc_test_constraint(struct Object *ob, struct bConstraint *cons) return; switch (data->type) { - case CONSTRAINT_IK_COPYPOSE: - case CONSTRAINT_IK_DISTANCE: - /* cartesian space constraint */ - break; + case CONSTRAINT_IK_COPYPOSE: + case CONSTRAINT_IK_DISTANCE: + /* cartesian space constraint */ + break; } } -- cgit v1.2.3