Welcome to mirror list, hosted at ThFree Co, Russian Federation.

git.blender.org/blender.git - Unnamed repository; edit this file 'description' to name the repository.
summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorCampbell Barton <ideasman42@gmail.com>2019-04-17 07:17:24 +0300
committerCampbell Barton <ideasman42@gmail.com>2019-04-17 07:21:24 +0300
commite12c08e8d170b7ca40f204a5b0423c23a9fbc2c1 (patch)
tree8cf3453d12edb177a218ef8009357518ec6cab6a /source/blender/ikplugin/intern/itasc_plugin.cpp
parentb3dabc200a4b0399ec6b81f2ff2730d07b44fcaa (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/intern/itasc_plugin.cpp')
-rw-r--r--source/blender/ikplugin/intern/itasc_plugin.cpp3358
1 files changed, 1727 insertions, 1631 deletions
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 &timestamp,
+ const iTaSC::Frame &current,
+ 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 &timestamp,
+ const iTaSC::Frame &current,
+ 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 &timestamp,
+ 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 &timestamp,
+ 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 &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;
- 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;
+ }
}