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
path: root/source/blender/ikplugin/intern/itasc_plugin.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'source/blender/ikplugin/intern/itasc_plugin.cpp')
-rw-r--r--source/blender/ikplugin/intern/itasc_plugin.cpp1053
1 files changed, 525 insertions, 528 deletions
diff --git a/source/blender/ikplugin/intern/itasc_plugin.cpp b/source/blender/ikplugin/intern/itasc_plugin.cpp
index 048dd955726..93934b41f28 100644
--- a/source/blender/ikplugin/intern/itasc_plugin.cpp
+++ b/source/blender/ikplugin/intern/itasc_plugin.cpp
@@ -21,7 +21,7 @@
* The Original Code is: all of this file.
*
* Original author: Benoit Bolsee
- * Contributor(s):
+ * Contributor(s):
*
* ***** END GPL LICENSE BLOCK *****
*/
@@ -30,7 +30,6 @@
* \ingroup ikplugin
*/
-
#include <stdlib.h>
#include <string.h>
#include <vector>
@@ -73,23 +72,22 @@ extern "C" {
bItasc DefIKParam;
// in case of animation mode, feedback and timestep is fixed
-#define ANIM_TIMESTEP 1.0
-#define ANIM_FEEDBACK 0.8
-#define ANIM_QMAX 0.52
+#define ANIM_TIMESTEP 1.0
+#define ANIM_FEEDBACK 0.8
+#define ANIM_QMAX 0.52
// Structure pointed by bPose.ikdata
// It contains everything needed to simulate the armatures
// There can be several simulation islands independent to each other
-struct IK_Data
-{
- struct IK_Scene* first;
+struct IK_Data {
+ struct IK_Scene *first;
};
typedef float Vector3[3];
typedef float Vector4[4];
struct IK_Target;
-typedef void (*ErrorCallback)(const iTaSC::ConstraintValues* values, unsigned int nvalues, IK_Target* iktarget);
+typedef void (*ErrorCallback)(const iTaSC::ConstraintValues *values, unsigned int nvalues, IK_Target *iktarget);
// one structure for each target in the scene
struct IK_Target
@@ -160,8 +158,7 @@ struct IK_Channel {
}
};
-struct IK_Scene
-{
+struct IK_Scene {
struct Scene *blscene;
IK_Scene* next;
int numchan; // number of channel in pchan
@@ -201,11 +198,11 @@ struct IK_Scene
// delete scene first
if (scene)
delete scene;
- for (std::vector<IK_Target*>::iterator it = targets.begin(); it != targets.end(); ++it)
+ for (std::vector<IK_Target *>::iterator it = targets.begin(); it != targets.end(); ++it)
delete (*it);
targets.clear();
if (channels)
- delete [] channels;
+ delete[] channels;
if (solver)
delete solver;
if (armature)
@@ -239,38 +236,38 @@ enum IK_SegmentAxis {
static int initialize_chain(Object *ob, bPoseChannel *pchan_tip, bConstraint *con)
{
- bPoseChannel *curchan, *pchan_root=NULL, *chanlist[256], **oldchan;
+ bPoseChannel *curchan, *pchan_root = NULL, *chanlist[256], **oldchan;
PoseTree *tree;
PoseTarget *target;
bKinematicConstraint *data;
- int a, t, segcount= 0, size, newsize, *oldparent, parent, rootbone, treecount;
+ int a, t, segcount = 0, size, newsize, *oldparent, parent, rootbone, treecount;
+
+ data = (bKinematicConstraint *)con->data;
- data=(bKinematicConstraint*)con->data;
-
/* exclude tip from chain? */
if (!(data->flag & CONSTRAINT_IK_TIP))
- pchan_tip= pchan_tip->parent;
-
+ pchan_tip = pchan_tip->parent;
+
rootbone = data->rootbone;
/* Find the chain's root & count the segments needed */
- for (curchan = pchan_tip; curchan; curchan=curchan->parent) {
+ for (curchan = pchan_tip; curchan; curchan = curchan->parent) {
pchan_root = curchan;
-
- if (++segcount > 255) // 255 is weak
+
+ if (++segcount > 255) // 255 is weak
break;
- if (segcount==rootbone) {
- // reached this end of the chain but if the chain is overlapping with a
+ if (segcount == rootbone) {
+ // reached this end of the chain but if the chain is overlapping with a
// previous one, we must go back up to the root of the other chain
if ((curchan->flag & POSE_CHAIN) && curchan->iktree.first == NULL) {
rootbone++;
continue;
}
- break;
+ break;
}
if (curchan->iktree.first != NULL)
- // Oh oh, there is already a chain starting from this channel and our chain is longer...
+ // Oh oh, there is already a chain starting from this channel and our chain is longer...
// Should handle this by moving the previous chain up to the beginning of our chain
// For now we just stop here
break;
@@ -280,86 +277,86 @@ static int initialize_chain(Object *ob, bPoseChannel *pchan_tip, bConstraint *co
if ((pchan_root->flag & POSE_CHAIN) && pchan_root->iktree.first == NULL) return 0;
// now that we know how many segment we have, set the flag
- for (rootbone = segcount, segcount = 0, curchan = pchan_tip; segcount < rootbone; segcount++, curchan=curchan->parent) {
- chanlist[segcount]=curchan;
+ for (rootbone = segcount, segcount = 0, curchan = pchan_tip; segcount < rootbone; segcount++, curchan = curchan->parent) {
+ chanlist[segcount] = curchan;
curchan->flag |= POSE_CHAIN;
}
/* setup the chain data */
/* create a target */
- target= (PoseTarget*)MEM_callocN(sizeof(PoseTarget), "posetarget");
- target->con= con;
+ target = (PoseTarget *)MEM_callocN(sizeof(PoseTarget), "posetarget");
+ target->con = con;
// by contruction there can be only one tree per channel and each channel can be part of at most one tree.
- tree = (PoseTree*)pchan_root->iktree.first;
+ tree = (PoseTree *)pchan_root->iktree.first;
- if (tree==NULL) {
+ if (tree == NULL) {
/* make new tree */
- tree= (PoseTree*)MEM_callocN(sizeof(PoseTree), "posetree");
+ tree = (PoseTree *)MEM_callocN(sizeof(PoseTree), "posetree");
- tree->iterations= data->iterations;
- tree->totchannel= segcount;
+ tree->iterations = data->iterations;
+ tree->totchannel = segcount;
tree->stretch = (data->flag & CONSTRAINT_IK_STRETCH);
-
- tree->pchan= (bPoseChannel**)MEM_callocN(segcount*sizeof(void*), "ik tree pchan");
- tree->parent= (int*)MEM_callocN(segcount*sizeof(int), "ik tree parent");
- for (a=0; a<segcount; a++) {
- tree->pchan[a]= chanlist[segcount-a-1];
- tree->parent[a]= a-1;
+
+ tree->pchan = (bPoseChannel **)MEM_callocN(segcount * sizeof(void *), "ik tree pchan");
+ tree->parent = (int *)MEM_callocN(segcount * sizeof(int), "ik tree parent");
+ for (a = 0; a < segcount; a++) {
+ tree->pchan[a] = chanlist[segcount - a - 1];
+ tree->parent[a] = a - 1;
}
- target->tip= segcount-1;
-
+ target->tip = segcount - 1;
+
/* AND! link the tree to the root */
BLI_addtail(&pchan_root->iktree, tree);
// new tree
treecount = 1;
}
else {
- tree->iterations= MAX2(data->iterations, tree->iterations);
- tree->stretch= tree->stretch && !(data->flag & CONSTRAINT_IK_STRETCH);
+ tree->iterations = MAX2(data->iterations, tree->iterations);
+ tree->stretch = tree->stretch && !(data->flag & CONSTRAINT_IK_STRETCH);
/* skip common pose channels and add remaining*/
- size= MIN2(segcount, tree->totchannel);
+ size = MIN2(segcount, tree->totchannel);
a = t = 0;
- while (a<size && t<tree->totchannel) {
+ 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)
+ 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++);
+ for (; a < size && t < tree->totchannel && tree->pchan[t] == chanlist[segcount - a - 1]; a++, t++) ;
}
- segcount= segcount-a;
- target->tip= tree->totchannel + segcount - 1;
+ segcount = segcount - a;
+ target->tip = tree->totchannel + segcount - 1;
if (segcount > 0) {
for (parent = a - 1; parent < tree->totchannel; parent++)
- if (tree->pchan[parent] == chanlist[segcount-1]->parent)
+ if (tree->pchan[parent] == chanlist[segcount - 1]->parent)
break;
-
+
/* shouldn't happen, but could with dependency cycles */
if (parent == tree->totchannel)
parent = a - 1;
/* resize array */
- newsize= tree->totchannel + segcount;
- oldchan= tree->pchan;
- oldparent= tree->parent;
-
- tree->pchan= (bPoseChannel**)MEM_callocN(newsize*sizeof(void*), "ik tree pchan");
- tree->parent= (int*)MEM_callocN(newsize*sizeof(int), "ik tree parent");
- memcpy(tree->pchan, oldchan, sizeof(void*)*tree->totchannel);
- memcpy(tree->parent, oldparent, sizeof(int)*tree->totchannel);
+ newsize = tree->totchannel + segcount;
+ oldchan = tree->pchan;
+ oldparent = tree->parent;
+
+ tree->pchan = (bPoseChannel **)MEM_callocN(newsize * sizeof(void *), "ik tree pchan");
+ tree->parent = (int *)MEM_callocN(newsize * sizeof(int), "ik tree parent");
+ memcpy(tree->pchan, oldchan, sizeof(void *) * tree->totchannel);
+ memcpy(tree->parent, oldparent, sizeof(int) * tree->totchannel);
MEM_freeN(oldchan);
MEM_freeN(oldparent);
/* add new pose channels at the end, in reverse order */
- for (a=0; a<segcount; a++) {
- tree->pchan[tree->totchannel+a]= chanlist[segcount-a-1];
- tree->parent[tree->totchannel+a]= tree->totchannel+a-1;
+ for (a = 0; a < segcount; a++) {
+ tree->pchan[tree->totchannel + a] = chanlist[segcount - a - 1];
+ tree->parent[tree->totchannel + a] = tree->totchannel + a - 1;
}
- tree->parent[tree->totchannel]= parent;
-
- tree->totchannel= newsize;
+ tree->parent[tree->totchannel] = parent;
+
+ tree->totchannel = newsize;
}
// reusing tree
treecount = 0;
@@ -381,7 +378,7 @@ static bool is_cartesian_constraint(bConstraint *con)
static bool constraint_valid(bConstraint *con)
{
- bKinematicConstraint* data=(bKinematicConstraint*)con->data;
+ bKinematicConstraint *data = (bKinematicConstraint *)con->data;
if (data->flag & CONSTRAINT_IK_AUTO)
return true;
@@ -389,9 +386,9 @@ static bool constraint_valid(bConstraint *con)
return false;
if (is_cartesian_constraint(con)) {
/* cartesian space constraint */
- if (data->tar==NULL)
+ if (data->tar == NULL)
return false;
- if (data->tar->type==OB_ARMATURE && data->subtarget[0]==0)
+ if (data->tar->type == OB_ARMATURE && data->subtarget[0] == 0)
return false;
}
return true;
@@ -404,8 +401,8 @@ int initialize_scene(Object *ob, bPoseChannel *pchan_tip)
/* find all IK constraints and validate them */
treecount = 0;
- for (con= (bConstraint *)pchan_tip->constraints.first; con; con= (bConstraint *)con->next) {
- if (con->type==CONSTRAINT_TYPE_KINEMATIC) {
+ for (con = (bConstraint *)pchan_tip->constraints.first; con; con = (bConstraint *)con->next) {
+ if (con->type == CONSTRAINT_TYPE_KINEMATIC) {
if (constraint_valid(con))
treecount += initialize_chain(ob, pchan_tip, con);
}
@@ -413,23 +410,23 @@ int initialize_scene(Object *ob, bPoseChannel *pchan_tip)
return treecount;
}
-static IK_Data* get_ikdata(bPose *pose)
+static IK_Data *get_ikdata(bPose *pose)
{
if (pose->ikdata)
- return (IK_Data*)pose->ikdata;
+ return (IK_Data *)pose->ikdata;
pose->ikdata = MEM_callocN(sizeof(IK_Data), "iTaSC ikdata");
// here init ikdata if needed
// now that we have scene, make sure the default param are initialized
if (!DefIKParam.iksolver)
BKE_pose_itasc_init(&DefIKParam);
- return (IK_Data*)pose->ikdata;
+ return (IK_Data *)pose->ikdata;
}
static double EulerAngleFromMatrix(const KDL::Rotation& R, int axis)
{
- double t = KDL::sqrt(R(0, 0)*R(0, 0) + R(0, 1)*R(0, 1));
+ double t = KDL::sqrt(R(0, 0) * R(0, 0) + R(0, 1) * R(0, 1));
- if (t > 16.0*KDL::epsilon) {
+ if (t > 16.0 * KDL::epsilon) {
if (axis == 0) return -KDL::atan2(R(1, 2), R(2, 2));
else if (axis == 1) return KDL::atan2(-R(0, 2), t);
else return -KDL::atan2(R(0, 1), R(0, 0));
@@ -447,7 +444,7 @@ static double ComputeTwist(const KDL::Rotation& R)
double qy = R(0, 2) - R(2, 0);
double qw = R(0, 0) + R(1, 1) + R(2, 2) + 1;
- double tau = 2*KDL::atan2(qy, qw);
+ double tau = 2 * KDL::atan2(qy, qw);
return tau;
}
@@ -457,102 +454,102 @@ static void RemoveEulerAngleFromMatrix(KDL::Rotation& R, double angle, int axis)
// compute twist parameter
KDL::Rotation T;
switch (axis) {
- case 0:
- T = KDL::Rotation::RotX(-angle);
- break;
- case 1:
- T = KDL::Rotation::RotY(-angle);
- break;
- case 2:
- T = KDL::Rotation::RotZ(-angle);
- break;
- default:
- return;
+ case 0:
+ T = KDL::Rotation::RotX(-angle);
+ break;
+ case 1:
+ T = KDL::Rotation::RotY(-angle);
+ break;
+ case 2:
+ T = KDL::Rotation::RotZ(-angle);
+ break;
+ default:
+ return;
}
// remove angle
- R = R*T;
+ R = R * T;
}
#if 0
static void GetEulerXZY(const KDL::Rotation& R, double& X, double& Z, double& Y)
{
- if (fabs(R(0, 1)) > 1.0 - KDL::epsilon ) {
+ if (fabs(R(0, 1)) > 1.0 - KDL::epsilon) {
X = -KDL::sign(R(0, 1)) * KDL::atan2(R(1, 2), R(1, 0));
Z = -KDL::sign(R(0, 1)) * KDL::PI / 2;
Y = 0.0;
}
else {
X = KDL::atan2(R(2, 1), R(1, 1));
- Z = KDL::atan2(-R(0, 1), KDL::sqrt( KDL::sqr(R(0, 0)) + KDL::sqr(R(0, 2))));
+ Z = KDL::atan2(-R(0, 1), KDL::sqrt(KDL::sqr(R(0, 0)) + KDL::sqr(R(0, 2))));
Y = KDL::atan2(R(0, 2), R(0, 0));
}
}
static void GetEulerXYZ(const KDL::Rotation& R, double& X, double& Y, double& Z)
{
- if (fabs(R(0, 2)) > 1.0 - KDL::epsilon ) {
+ if (fabs(R(0, 2)) > 1.0 - KDL::epsilon) {
X = KDL::sign(R(0, 2)) * KDL::atan2(-R(1, 0), R(1, 1));
Y = KDL::sign(R(0, 2)) * KDL::PI / 2;
Z = 0.0;
}
else {
X = KDL::atan2(-R(1, 2), R(2, 2));
- Y = KDL::atan2(R(0, 2), KDL::sqrt( KDL::sqr(R(0, 0)) + KDL::sqr(R(0, 1))));
+ Y = KDL::atan2(R(0, 2), KDL::sqrt(KDL::sqr(R(0, 0)) + KDL::sqr(R(0, 1))));
Z = KDL::atan2(-R(0, 1), R(0, 0));
}
}
#endif
-static void GetJointRotation(KDL::Rotation& boneRot, int type, double* rot)
+static void GetJointRotation(KDL::Rotation& boneRot, int type, double *rot)
{
switch (type & ~IK_TRANSY) {
- default:
- // fixed bone, no joint
- break;
- case IK_XDOF:
- // RX only, get the X rotation
- rot[0] = EulerAngleFromMatrix(boneRot, 0);
- break;
- case IK_YDOF:
- // RY only, get the Y rotation
- rot[0] = ComputeTwist(boneRot);
- break;
- case IK_ZDOF:
- // RZ only, get the Z rotation
- rot[0] = EulerAngleFromMatrix(boneRot, 2);
- break;
- case IK_XDOF|IK_YDOF:
- rot[1] = ComputeTwist(boneRot);
- RemoveEulerAngleFromMatrix(boneRot, rot[1], 1);
- rot[0] = EulerAngleFromMatrix(boneRot, 0);
- break;
- case IK_SWING:
- // RX+RZ
- boneRot.GetXZRot().GetValue(rot);
- break;
- case IK_YDOF|IK_ZDOF:
- // RZ+RY
- rot[1] = ComputeTwist(boneRot);
- RemoveEulerAngleFromMatrix(boneRot, rot[1], 1);
- rot[0] = EulerAngleFromMatrix(boneRot, 2);
- break;
- case IK_SWING|IK_YDOF:
- rot[2] = ComputeTwist(boneRot);
- RemoveEulerAngleFromMatrix(boneRot, rot[2], 1);
- boneRot.GetXZRot().GetValue(rot);
- break;
- case IK_REVOLUTE:
- boneRot.GetRot().GetValue(rot);
- break;
+ default:
+ // fixed bone, no joint
+ break;
+ case IK_XDOF:
+ // RX only, get the X rotation
+ rot[0] = EulerAngleFromMatrix(boneRot, 0);
+ break;
+ case IK_YDOF:
+ // RY only, get the Y rotation
+ rot[0] = ComputeTwist(boneRot);
+ break;
+ case IK_ZDOF:
+ // RZ only, get the Z rotation
+ rot[0] = EulerAngleFromMatrix(boneRot, 2);
+ break;
+ case IK_XDOF | IK_YDOF:
+ rot[1] = ComputeTwist(boneRot);
+ RemoveEulerAngleFromMatrix(boneRot, rot[1], 1);
+ rot[0] = EulerAngleFromMatrix(boneRot, 0);
+ break;
+ case IK_SWING:
+ // RX+RZ
+ boneRot.GetXZRot().GetValue(rot);
+ break;
+ case IK_YDOF | IK_ZDOF:
+ // RZ+RY
+ rot[1] = ComputeTwist(boneRot);
+ RemoveEulerAngleFromMatrix(boneRot, rot[1], 1);
+ rot[0] = EulerAngleFromMatrix(boneRot, 2);
+ break;
+ case IK_SWING | IK_YDOF:
+ rot[2] = ComputeTwist(boneRot);
+ RemoveEulerAngleFromMatrix(boneRot, rot[2], 1);
+ boneRot.GetXZRot().GetValue(rot);
+ break;
+ case IK_REVOLUTE:
+ boneRot.GetRot().GetValue(rot);
+ break;
}
}
static bool target_callback(const iTaSC::Timestamp& timestamp, const iTaSC::Frame& current, iTaSC::Frame& next, void *param)
{
- IK_Target* target = (IK_Target*)param;
+ IK_Target *target = (IK_Target *)param;
// compute next target position
// get target matrix from constraint.
- bConstraint* constraint = (bConstraint*)target->blenderConstraint;
+ bConstraint *constraint = (bConstraint *)target->blenderConstraint;
float tarmat[4][4];
get_constraint_target_matrix(target->blscene, constraint, 0, CONSTRAINT_OBTYPE_OBJECT, target->owner, tarmat, 1.0);
@@ -564,14 +561,14 @@ static bool target_callback(const iTaSC::Timestamp& timestamp, const iTaSC::Fram
// eeRest is relative to the reference frame of the IK root
// get this frame in world reference
float restmat[4][4];
- bPoseChannel* pchan = target->rootChannel;
+ bPoseChannel *pchan = target->rootChannel;
if (pchan->parent) {
pchan = pchan->parent;
float chanmat[4][4];
copy_m4_m4(chanmat, pchan->pose_mat);
copy_v3_v3(chanmat[3], pchan->pose_tail);
mul_serie_m4(restmat, target->owner->obmat, chanmat, target->eeRest, NULL, NULL, NULL, NULL, NULL);
- }
+ }
else {
mult_m4_m4m4(restmat, target->owner->obmat, target->eeRest);
}
@@ -584,13 +581,13 @@ static bool target_callback(const iTaSC::Timestamp& timestamp, const iTaSC::Fram
static bool base_callback(const iTaSC::Timestamp& timestamp, const iTaSC::Frame& current, iTaSC::Frame& next, void *param)
{
- IK_Scene* ikscene = (IK_Scene*)param;
+ IK_Scene *ikscene = (IK_Scene *)param;
// compute next armature base pose
- // algorithm:
+ // algorithm:
// ikscene->pchan[0] is the root channel of the tree
// if it has a parent, get the pose matrix from it and replace [3] by parent pchan->tail
// then multiply by the armature matrix to get ikscene->armature base position
- bPoseChannel* pchan = ikscene->channels[0].pchan;
+ bPoseChannel *pchan = ikscene->channels[0].pchan;
float rootmat[4][4];
if (pchan->parent) {
pchan = pchan->parent;
@@ -602,7 +599,7 @@ static bool base_callback(const iTaSC::Timestamp& timestamp, const iTaSC::Frame&
// iTaSC armature is scaled to object scale, scale the base frame too
ikscene->baseFrame.p *= ikscene->blScale;
mult_m4_m4m4(rootmat, ikscene->blArmature->obmat, chanmat);
- }
+ }
else {
copy_m4_m4(rootmat, ikscene->blArmature->obmat);
ikscene->baseFrame = iTaSC::F_identity;
@@ -611,11 +608,11 @@ static bool base_callback(const iTaSC::Timestamp& timestamp, const iTaSC::Frame&
// if there is a polar target (only during solving otherwise we don't have end efffector)
if (ikscene->polarConstraint && timestamp.update) {
// compute additional rotation of base frame so that armature follows the polar target
- float imat[4][4]; // IK tree base inverse matrix
- float polemat[4][4]; // polar target in IK tree base frame
- float goalmat[4][4]; // target in IK tree base frame
- float mat[4][4]; // temp matrix
- bKinematicConstraint* poledata = (bKinematicConstraint*)ikscene->polarConstraint->data;
+ float imat[4][4]; // IK tree base inverse matrix
+ float polemat[4][4]; // polar target in IK tree base frame
+ float goalmat[4][4]; // target in IK tree base frame
+ float mat[4][4]; // temp matrix
+ bKinematicConstraint *poledata = (bKinematicConstraint *)ikscene->polarConstraint->data;
invert_m4_m4(imat, rootmat);
// polar constraint imply only one target
@@ -642,48 +639,48 @@ static bool base_callback(const iTaSC::Timestamp& timestamp, const iTaSC::Frame&
KDL::Vector rootz = rootframe.M.UnitZ();
// and compute root bone head
double q_rest[3], q[3], length;
- const KDL::Joint* joint;
- const KDL::Frame* tip;
+ const KDL::Joint *joint;
+ const KDL::Frame *tip;
ikscene->armature->getSegment(rootchan.tail, 3, joint, q_rest[0], q[0], tip);
length = (joint->getType() == KDL::Joint::TransY) ? q[0] : tip->p(1);
- KDL::Vector rootpos = rootframe.p - length*rootframe.M.UnitY();
+ KDL::Vector rootpos = rootframe.p - length *rootframe.M.UnitY();
- // compute main directions
+ // compute main directions
KDL::Vector dir = KDL::Normalize(endpos - rootpos);
- KDL::Vector poledir = KDL::Normalize(goalpos-rootpos);
+ KDL::Vector poledir = KDL::Normalize(goalpos - rootpos);
// compute up directions
- KDL::Vector poleup = KDL::Normalize(polepos-rootpos);
- KDL::Vector up = rootx*KDL::cos(poledata->poleangle) + rootz*KDL::sin(poledata->poleangle);
+ KDL::Vector poleup = KDL::Normalize(polepos - rootpos);
+ KDL::Vector up = rootx * KDL::cos(poledata->poleangle) + rootz *KDL::sin(poledata->poleangle);
// from which we build rotation matrix
KDL::Rotation endrot, polerot;
// for the armature, using the root bone orientation
- KDL::Vector x = KDL::Normalize(dir*up);
+ KDL::Vector x = KDL::Normalize(dir * up);
endrot.UnitX(x);
- endrot.UnitY(KDL::Normalize(x*dir));
+ endrot.UnitY(KDL::Normalize(x * dir));
endrot.UnitZ(-dir);
- // for the polar target
- x = KDL::Normalize(poledir*poleup);
+ // for the polar target
+ x = KDL::Normalize(poledir * poleup);
polerot.UnitX(x);
- polerot.UnitY(KDL::Normalize(x*poledir));
+ polerot.UnitY(KDL::Normalize(x * poledir));
polerot.UnitZ(-poledir);
// the difference between the two is the rotation we want to apply
- KDL::Rotation result(polerot*endrot.Inverse());
+ KDL::Rotation result(polerot * endrot.Inverse());
// apply on base frame as this is an artificial additional rotation
- next.M = next.M*result;
- ikscene->baseFrame.M = ikscene->baseFrame.M*result;
+ next.M = next.M * result;
+ ikscene->baseFrame.M = ikscene->baseFrame.M * result;
}
return true;
}
-static bool copypose_callback(const iTaSC::Timestamp& timestamp, iTaSC::ConstraintValues* const _values, unsigned int _nvalues, void* _param)
+static bool copypose_callback(const iTaSC::Timestamp& timestamp, iTaSC::ConstraintValues *const _values, unsigned int _nvalues, void *_param)
{
- IK_Target* iktarget =(IK_Target*)_param;
+ IK_Target *iktarget = (IK_Target *)_param;
bKinematicConstraint *condata = (bKinematicConstraint *)iktarget->blenderConstraint->data;
- iTaSC::ConstraintValues* values = _values;
- bItasc* ikparam = (bItasc*) iktarget->owner->pose->ikparam;
+ iTaSC::ConstraintValues *values = _values;
+ bItasc *ikparam = (bItasc *) iktarget->owner->pose->ikparam;
// we need default parameters
- if (!ikparam)
+ if (!ikparam)
ikparam = &DefIKParam;
if (iktarget->blenderConstraint->flag & CONSTRAINT_OFF) {
@@ -702,14 +699,14 @@ static bool copypose_callback(const iTaSC::Timestamp& timestamp, iTaSC::Constrai
if (iktarget->controlType & iTaSC::CopyPose::CTL_POSITION) {
// update error
values->alpha = condata->weight;
- values->action = iTaSC::ACT_ALPHA|iTaSC::ACT_FEEDBACK;
+ values->action = iTaSC::ACT_ALPHA | iTaSC::ACT_FEEDBACK;
values->feedback = (iktarget->simulation) ? ikparam->feedback : ANIM_FEEDBACK;
values++;
}
if (iktarget->controlType & iTaSC::CopyPose::CTL_ROTATION) {
// update error
values->alpha = condata->orientweight;
- values->action = iTaSC::ACT_ALPHA|iTaSC::ACT_FEEDBACK;
+ values->action = iTaSC::ACT_ALPHA | iTaSC::ACT_FEEDBACK;
values->feedback = (iktarget->simulation) ? ikparam->feedback : ANIM_FEEDBACK;
values++;
}
@@ -717,36 +714,36 @@ static bool copypose_callback(const iTaSC::Timestamp& timestamp, iTaSC::Constrai
return true;
}
-static void copypose_error(const iTaSC::ConstraintValues* values, unsigned int nvalues, IK_Target* iktarget)
+static void copypose_error(const iTaSC::ConstraintValues *values, unsigned int nvalues, IK_Target *iktarget)
{
- iTaSC::ConstraintSingleValue* value;
+ iTaSC::ConstraintSingleValue *value;
double error;
int i;
if (iktarget->controlType & iTaSC::CopyPose::CTL_POSITION) {
// update error
- for (i=0, error=0.0, value=values->values; i<values->number; ++i, ++value)
+ for (i = 0, error = 0.0, value = values->values; i < values->number; ++i, ++value)
error += KDL::sqr(value->y - value->yd);
iktarget->blenderConstraint->lin_error = (float)KDL::sqrt(error);
values++;
}
if (iktarget->controlType & iTaSC::CopyPose::CTL_ROTATION) {
// update error
- for (i=0, error=0.0, value=values->values; i<values->number; ++i, ++value)
+ for (i = 0, error = 0.0, value = values->values; i < values->number; ++i, ++value)
error += KDL::sqr(value->y - value->yd);
iktarget->blenderConstraint->rot_error = (float)KDL::sqrt(error);
values++;
}
}
-static bool distance_callback(const iTaSC::Timestamp& timestamp, iTaSC::ConstraintValues* const _values, unsigned int _nvalues, void* _param)
+static bool distance_callback(const iTaSC::Timestamp& timestamp, iTaSC::ConstraintValues *const _values, unsigned int _nvalues, void *_param)
{
- IK_Target* iktarget =(IK_Target*)_param;
+ IK_Target *iktarget = (IK_Target *)_param;
bKinematicConstraint *condata = (bKinematicConstraint *)iktarget->blenderConstraint->data;
- iTaSC::ConstraintValues* values = _values;
- bItasc* ikparam = (bItasc*) iktarget->owner->pose->ikparam;
+ iTaSC::ConstraintValues *values = _values;
+ bItasc *ikparam = (bItasc *) iktarget->owner->pose->ikparam;
// we need default parameters
- if (!ikparam)
+ if (!ikparam)
ikparam = &DefIKParam;
// update weight according to mode
@@ -755,30 +752,30 @@ static bool distance_callback(const iTaSC::Timestamp& timestamp, iTaSC::Constrai
}
else {
switch (condata->mode) {
- case LIMITDIST_INSIDE:
- values->alpha = (values->values[0].y > condata->dist) ? condata->weight : 0.0;
- break;
- case LIMITDIST_OUTSIDE:
- values->alpha = (values->values[0].y < condata->dist) ? condata->weight : 0.0;
- break;
- default:
- values->alpha = condata->weight;
- break;
- }
- if (!timestamp.substep) {
- // only update value on first timestep
- switch (condata->mode) {
case LIMITDIST_INSIDE:
- values->values[0].yd = condata->dist*0.95;
+ values->alpha = (values->values[0].y > condata->dist) ? condata->weight : 0.0;
break;
case LIMITDIST_OUTSIDE:
- values->values[0].yd = condata->dist*1.05;
+ values->alpha = (values->values[0].y < condata->dist) ? condata->weight : 0.0;
break;
default:
- values->values[0].yd = condata->dist;
+ values->alpha = condata->weight;
break;
+ }
+ if (!timestamp.substep) {
+ // only update value on first timestep
+ switch (condata->mode) {
+ case LIMITDIST_INSIDE:
+ values->values[0].yd = condata->dist * 0.95;
+ break;
+ case LIMITDIST_OUTSIDE:
+ values->values[0].yd = condata->dist * 1.05;
+ break;
+ default:
+ values->values[0].yd = condata->dist;
+ break;
}
- values->values[0].action = iTaSC::ACT_VALUE|iTaSC::ACT_FEEDBACK;
+ values->values[0].action = iTaSC::ACT_VALUE | iTaSC::ACT_FEEDBACK;
values->feedback = (iktarget->simulation) ? ikparam->feedback : ANIM_FEEDBACK;
}
}
@@ -786,16 +783,16 @@ static bool distance_callback(const iTaSC::Timestamp& timestamp, iTaSC::Constrai
return true;
}
-static void distance_error(const iTaSC::ConstraintValues* values, unsigned int _nvalues, IK_Target* iktarget)
+static void distance_error(const iTaSC::ConstraintValues *values, unsigned int _nvalues, IK_Target *iktarget)
{
iktarget->blenderConstraint->lin_error = (float)(values->values[0].y - values->values[0].yd);
}
-static bool joint_callback(const iTaSC::Timestamp& timestamp, iTaSC::ConstraintValues* const _values, unsigned int _nvalues, void* _param)
+static bool joint_callback(const iTaSC::Timestamp& timestamp, iTaSC::ConstraintValues *const _values, unsigned int _nvalues, void *_param)
{
- IK_Channel* ikchan = (IK_Channel*)_param;
- bItasc* ikparam = (bItasc*)ikchan->owner->pose->ikparam;
- bPoseChannel* chan = ikchan->pchan;
+ IK_Channel *ikchan = (IK_Channel *)_param;
+ bItasc *ikparam = (bItasc *)ikchan->owner->pose->ikparam;
+ bPoseChannel *chan = ikchan->pchan;
int dof;
// a channel can be splitted into multiple joints, so we get called multiple
@@ -808,11 +805,11 @@ static bool joint_callback(const iTaSC::Timestamp& timestamp, iTaSC::ConstraintV
if (chan->rotmode > 0) {
/* euler rotations (will cause gimble lock, but this can be alleviated a bit with rotation orders) */
- eulO_to_mat3( rmat, chan->eul, chan->rotmode);
+ eulO_to_mat3(rmat, chan->eul, chan->rotmode);
}
else if (chan->rotmode == ROT_MODE_AXISANGLE) {
/* axis-angle - stored in quaternion data, but not really that great for 3D-changing orientations */
- axis_angle_to_mat3( rmat, &chan->quat[1], chan->quat[0]);
+ axis_angle_to_mat3(rmat, &chan->quat[1], chan->quat[0]);
}
else {
/* quats are normalised before use to eliminate scaling issues */
@@ -820,45 +817,45 @@ static bool joint_callback(const iTaSC::Timestamp& timestamp, iTaSC::ConstraintV
quat_to_mat3(rmat, chan->quat);
}
KDL::Rotation jointRot(
- rmat[0][0], rmat[1][0], rmat[2][0],
- rmat[0][1], rmat[1][1], rmat[2][1],
- rmat[0][2], rmat[1][2], rmat[2][2]);
+ rmat[0][0], rmat[1][0], rmat[2][0],
+ rmat[0][1], rmat[1][1], rmat[2][1],
+ rmat[0][2], rmat[1][2], rmat[2][2]);
GetJointRotation(jointRot, ikchan->jointType, ikchan->jointValue);
ikchan->jointValid = 1;
}
// determine which part of jointValue is used for this joint
// closely related to the way the joints are defined
switch (ikchan->jointType & ~IK_TRANSY) {
- case IK_XDOF:
- case IK_YDOF:
- case IK_ZDOF:
- dof = 0;
- break;
- case IK_XDOF|IK_YDOF:
- // X + Y
- dof = (_values[0].id == iTaSC::Armature::ID_JOINT_RX) ? 0 : 1;
- break;
- case IK_SWING:
- // XZ
- dof = 0;
- break;
- case IK_YDOF|IK_ZDOF:
- // Z + Y
- dof = (_values[0].id == iTaSC::Armature::ID_JOINT_RZ) ? 0 : 1;
- break;
- case IK_SWING|IK_YDOF:
- // XZ + Y
- dof = (_values[0].id == iTaSC::Armature::ID_JOINT_RY) ? 2 : 0;
- break;
- case IK_REVOLUTE:
- dof = 0;
- break;
- default:
- dof = -1;
- break;
+ case IK_XDOF:
+ case IK_YDOF:
+ case IK_ZDOF:
+ dof = 0;
+ break;
+ case IK_XDOF | IK_YDOF:
+ // X + Y
+ dof = (_values[0].id == iTaSC::Armature::ID_JOINT_RX) ? 0 : 1;
+ break;
+ case IK_SWING:
+ // XZ
+ dof = 0;
+ break;
+ case IK_YDOF | IK_ZDOF:
+ // Z + Y
+ dof = (_values[0].id == iTaSC::Armature::ID_JOINT_RZ) ? 0 : 1;
+ break;
+ case IK_SWING | IK_YDOF:
+ // XZ + Y
+ dof = (_values[0].id == iTaSC::Armature::ID_JOINT_RY) ? 2 : 0;
+ break;
+ case IK_REVOLUTE:
+ dof = 0;
+ break;
+ default:
+ dof = -1;
+ break;
}
if (dof >= 0) {
- for (unsigned int i=0; i<_nvalues; i++, dof++) {
+ for (unsigned int i = 0; i < _nvalues; i++, dof++) {
_values[i].values[0].yd = ikchan->jointValue[dof];
_values[i].alpha = chan->ikrotweight;
_values[i].feedback = ikparam->feedback;
@@ -875,30 +872,30 @@ static int convert_channels(IK_Scene *ikscene, PoseTree *tree)
int a, flag, njoint;
njoint = 0;
- for (a=0, ikchan = ikscene->channels; a<ikscene->numchan; ++a, ++ikchan) {
- pchan= tree->pchan[a];
+ for (a = 0, ikchan = ikscene->channels; a < ikscene->numchan; ++a, ++ikchan) {
+ pchan = tree->pchan[a];
ikchan->pchan = pchan;
- ikchan->parent = (a>0) ? tree->parent[a] : -1;
+ ikchan->parent = (a > 0) ? tree->parent[a] : -1;
ikchan->owner = ikscene->blArmature;
-
+
/* set DoF flag */
flag = 0;
if (!(pchan->ikflag & BONE_IK_NO_XDOF) && !(pchan->ikflag & BONE_IK_NO_XDOF_TEMP) &&
- (!(pchan->ikflag & BONE_IK_XLIMIT) || pchan->limitmin[0]<0.f || pchan->limitmax[0]>0.f))
+ (!(pchan->ikflag & BONE_IK_XLIMIT) || pchan->limitmin[0] < 0.f || pchan->limitmax[0] > 0.f))
{
flag |= IK_XDOF;
}
if (!(pchan->ikflag & BONE_IK_NO_YDOF) && !(pchan->ikflag & BONE_IK_NO_YDOF_TEMP) &&
- (!(pchan->ikflag & BONE_IK_YLIMIT) || pchan->limitmin[1]<0.f || pchan->limitmax[1]>0.f))
+ (!(pchan->ikflag & BONE_IK_YLIMIT) || pchan->limitmin[1] < 0.f || pchan->limitmax[1] > 0.f))
{
flag |= IK_YDOF;
}
if (!(pchan->ikflag & BONE_IK_NO_ZDOF) && !(pchan->ikflag & BONE_IK_NO_ZDOF_TEMP) &&
- (!(pchan->ikflag & BONE_IK_ZLIMIT) || pchan->limitmin[2]<0.f || pchan->limitmax[2]>0.f))
+ (!(pchan->ikflag & BONE_IK_ZLIMIT) || pchan->limitmin[2] < 0.f || pchan->limitmax[2] > 0.f))
{
flag |= IK_ZDOF;
}
-
+
if (tree->stretch && (pchan->ikstretch > 0.0)) {
flag |= IK_TRANSY;
}
@@ -929,49 +926,49 @@ static int convert_channels(IK_Scene *ikscene, PoseTree *tree)
* bone length is computed from bone->length multiplied by the scaling factor of
* the armature. Non-uniform scaling will give bad result!
*/
- switch (flag & (IK_XDOF|IK_YDOF|IK_ZDOF)) {
- default:
- ikchan->jointType = 0;
- ikchan->ndof = 0;
- break;
- case IK_XDOF:
- // RX only, get the X rotation
- ikchan->jointType = IK_XDOF;
- ikchan->ndof = 1;
- break;
- case IK_YDOF:
- // RY only, get the Y rotation
- ikchan->jointType = IK_YDOF;
- ikchan->ndof = 1;
- break;
- case IK_ZDOF:
- // RZ only, get the Zz rotation
- ikchan->jointType = IK_ZDOF;
- ikchan->ndof = 1;
- break;
- case IK_XDOF|IK_YDOF:
- ikchan->jointType = IK_XDOF|IK_YDOF;
- ikchan->ndof = 2;
- break;
- case IK_XDOF|IK_ZDOF:
- // RX+RZ
- ikchan->jointType = IK_SWING;
- ikchan->ndof = 2;
- break;
- case IK_YDOF|IK_ZDOF:
- // RZ+RY
- ikchan->jointType = IK_ZDOF|IK_YDOF;
- ikchan->ndof = 2;
- break;
- case IK_XDOF|IK_YDOF|IK_ZDOF:
- // spherical joint
- if (pchan->ikflag & (BONE_IK_XLIMIT|BONE_IK_YLIMIT|BONE_IK_ZLIMIT))
- // decompose in a Swing+RotY joint
- ikchan->jointType = IK_SWING|IK_YDOF;
- else
- ikchan->jointType = IK_REVOLUTE;
- ikchan->ndof = 3;
- break;
+ switch (flag & (IK_XDOF | IK_YDOF | IK_ZDOF)) {
+ default:
+ ikchan->jointType = 0;
+ ikchan->ndof = 0;
+ break;
+ case IK_XDOF:
+ // RX only, get the X rotation
+ ikchan->jointType = IK_XDOF;
+ ikchan->ndof = 1;
+ break;
+ case IK_YDOF:
+ // RY only, get the Y rotation
+ ikchan->jointType = IK_YDOF;
+ ikchan->ndof = 1;
+ break;
+ case IK_ZDOF:
+ // RZ only, get the Zz rotation
+ ikchan->jointType = IK_ZDOF;
+ ikchan->ndof = 1;
+ break;
+ case IK_XDOF | IK_YDOF:
+ ikchan->jointType = IK_XDOF | IK_YDOF;
+ ikchan->ndof = 2;
+ break;
+ case IK_XDOF | IK_ZDOF:
+ // RX+RZ
+ ikchan->jointType = IK_SWING;
+ ikchan->ndof = 2;
+ break;
+ case IK_YDOF | IK_ZDOF:
+ // RZ+RY
+ ikchan->jointType = IK_ZDOF | IK_YDOF;
+ ikchan->ndof = 2;
+ break;
+ case IK_XDOF | IK_YDOF | IK_ZDOF:
+ // spherical joint
+ if (pchan->ikflag & (BONE_IK_XLIMIT | BONE_IK_YLIMIT | BONE_IK_ZLIMIT))
+ // decompose in a Swing+RotY joint
+ ikchan->jointType = IK_SWING | IK_YDOF;
+ else
+ ikchan->jointType = IK_REVOLUTE;
+ ikchan->ndof = 3;
+ break;
}
if (flag & IK_TRANSY) {
ikchan->jointType |= IK_TRANSY;
@@ -992,8 +989,8 @@ static void convert_pose(IK_Scene *ikscene)
bPoseChannel *pchan;
IK_Channel *ikchan;
Bone *bone;
- float rmat[4][4]; // rest pose of bone with parent taken into account
- float bmat[4][4]; // difference
+ float rmat[4][4]; // rest pose of bone with parent taken into account
+ float bmat[4][4]; // difference
float scale;
double *rot;
int a, joint;
@@ -1001,10 +998,10 @@ static void convert_pose(IK_Scene *ikscene)
// assume uniform scaling and take Y scale as general scale for the armature
scale = len_v3(ikscene->blArmature->obmat[1]);
rot = ikscene->jointArray(0);
- for (joint=a=0, ikchan = ikscene->channels; a<ikscene->numchan && joint<ikscene->numjoint; ++a, ++ikchan) {
- pchan= ikchan->pchan;
- bone= pchan->bone;
-
+ for (joint = a = 0, ikchan = ikscene->channels; a < ikscene->numchan && joint < ikscene->numjoint; ++a, ++ikchan) {
+ pchan = ikchan->pchan;
+ bone = pchan->bone;
+
if (pchan->parent) {
unit_m4(bmat);
mul_m4_m4m3(bmat, pchan->parent->pose_mat, bone->bone_mat);
@@ -1018,9 +1015,9 @@ static void convert_pose(IK_Scene *ikscene)
boneRot.setValue(bmat[0]);
GetJointRotation(boneRot, ikchan->jointType, rot);
if (ikchan->jointType & IK_TRANSY) {
- // compute actual length
- rot[ikchan->ndof-1] = len_v3v3(pchan->pose_tail, pchan->pose_head) * scale;
- }
+ // compute actual length
+ rot[ikchan->ndof - 1] = len_v3v3(pchan->pose_tail, pchan->pose_head) * scale;
+ }
rot += ikchan->ndof;
joint += ikchan->ndof;
}
@@ -1038,32 +1035,32 @@ static void BKE_pose_rest(IK_Scene *ikscene)
// assume uniform scaling and take Y scale as general scale for the armature
scale = len_v3(ikscene->blArmature->obmat[1]);
- // rest pose is 0
+ // rest pose is 0
SetToZero(ikscene->jointArray);
// except for transY joints
rot = ikscene->jointArray(0);
- for (joint=a=0, ikchan = ikscene->channels; a<ikscene->numchan && joint<ikscene->numjoint; ++a, ++ikchan) {
- pchan= ikchan->pchan;
- bone= pchan->bone;
+ for (joint = a = 0, ikchan = ikscene->channels; a < ikscene->numchan && joint < ikscene->numjoint; ++a, ++ikchan) {
+ pchan = ikchan->pchan;
+ bone = pchan->bone;
if (ikchan->jointType & IK_TRANSY)
- rot[ikchan->ndof-1] = bone->length*scale;
+ rot[ikchan->ndof - 1] = bone->length * scale;
rot += ikchan->ndof;
joint += ikchan->ndof;
}
}
-static IK_Scene* convert_tree(Scene *blscene, Object *ob, bPoseChannel *pchan)
+static IK_Scene *convert_tree(Scene *blscene, Object *ob, bPoseChannel *pchan)
{
- PoseTree *tree = (PoseTree*)pchan->iktree.first;
+ PoseTree *tree = (PoseTree *)pchan->iktree.first;
PoseTarget *target;
bKinematicConstraint *condata;
bConstraint *polarcon;
bItasc *ikparam;
- iTaSC::Armature* arm;
- iTaSC::Scene* scene;
- IK_Scene* ikscene;
- IK_Channel* ikchan;
+ iTaSC::Armature *arm;
+ iTaSC::Scene *scene;
+ IK_Scene *ikscene;
+ IK_Channel *ikchan;
KDL::Frame initPose;
KDL::Rotation boneRot;
Bone *bone;
@@ -1084,7 +1081,7 @@ static IK_Scene* convert_tree(Scene *blscene, Object *ob, bPoseChannel *pchan)
ikscene->numchan = tree->totchannel;
ikscene->armature = arm;
ikscene->scene = scene;
- ikparam = (bItasc*)ob->pose->ikparam;
+ ikparam = (bItasc *)ob->pose->ikparam;
ingame = (ob->pose->flag & POSE_GAME_ENGINE);
if (!ikparam) {
// you must have our own copy
@@ -1096,7 +1093,7 @@ static IK_Scene* convert_tree(Scene *blscene, Object *ob, bPoseChannel *pchan)
// shorter than in animation => move to auto step automatically and set
// the target substep duration via min/max
if (!(ikparam->flag & ITASC_AUTO_STEP)) {
- float timestep = blscene->r.frs_sec_base/blscene->r.frs_sec;
+ float timestep = blscene->r.frs_sec_base / blscene->r.frs_sec;
if (ikparam->numstep > 0)
timestep /= ikparam->numstep;
// with equal min and max, the algorythm will take this step and the indicative substep most of the time
@@ -1109,24 +1106,24 @@ static IK_Scene* convert_tree(Scene *blscene, Object *ob, bPoseChannel *pchan)
ikscene->cache = new iTaSC::Cache();
switch (ikparam->solver) {
- case ITASC_SOLVER_SDLS:
- ikscene->solver = new iTaSC::WSDLSSolver();
- break;
- case ITASC_SOLVER_DLS:
- ikscene->solver = new iTaSC::WDLSSolver();
- break;
- default:
- delete ikscene;
- return NULL;
+ case ITASC_SOLVER_SDLS:
+ ikscene->solver = new iTaSC::WSDLSSolver();
+ break;
+ case ITASC_SOLVER_DLS:
+ ikscene->solver = new iTaSC::WDLSSolver();
+ break;
+ default:
+ delete ikscene;
+ return NULL;
}
ikscene->blArmature = ob;
// assume uniform scaling and take Y scale as general scale for the armature
ikscene->blScale = len_v3(ob->obmat[1]);
- ikscene->blInvScale = (ikscene->blScale < KDL::epsilon) ? 0.0f : 1.0f/ikscene->blScale;
+ ikscene->blInvScale = (ikscene->blScale < KDL::epsilon) ? 0.0f : 1.0f / ikscene->blScale;
- std::string joint;
- std::string root("root");
- std::string parent;
+ std::string joint;
+ std::string root("root");
+ std::string parent;
std::vector<double> weights;
double weight[3];
// build the array of joints corresponding to the IK chain
@@ -1141,22 +1138,22 @@ static IK_Scene* convert_tree(Scene *blscene, Object *ob, bPoseChannel *pchan)
BKE_pose_rest(ikscene);
}
rot = ikscene->jointArray(0);
- for (a=0, ikchan = ikscene->channels; a<tree->totchannel; ++a, ++ikchan) {
- pchan= ikchan->pchan;
- bone= pchan->bone;
+ for (a = 0, ikchan = ikscene->channels; a < tree->totchannel; ++a, ++ikchan) {
+ pchan = ikchan->pchan;
+ bone = pchan->bone;
KDL::Frame tip(iTaSC::F_identity);
Vector3 *fl = bone->bone_mat;
KDL::Rotation brot(
- fl[0][0], fl[1][0], fl[2][0],
- fl[0][1], fl[1][1], fl[2][1],
- fl[0][2], fl[1][2], fl[2][2]);
+ fl[0][0], fl[1][0], fl[2][0],
+ fl[0][1], fl[1][1], fl[2][1],
+ fl[0][2], fl[1][2], fl[2][2]);
KDL::Vector bpos(bone->head[0], bone->head[1], bone->head[2]);
bpos *= ikscene->blScale;
KDL::Frame head(brot, bpos);
-
+
// rest pose length of the bone taking scaling into account
- length= bone->length*ikscene->blScale;
+ length = bone->length * ikscene->blScale;
parent = (a > 0) ? ikscene->channels[tree->parent[a]].tail : root;
// first the fixed segment to the bone head
if (head.p.Norm() > KDL::epsilon || head.M.GetRot().Norm() > KDL::epsilon) {
@@ -1170,95 +1167,95 @@ static IK_Scene* convert_tree(Scene *blscene, Object *ob, bPoseChannel *pchan)
tip.p[1] = length;
}
joint = bone->name;
- weight[0] = (1.0-pchan->stiffness[0]);
- weight[1] = (1.0-pchan->stiffness[1]);
- weight[2] = (1.0-pchan->stiffness[2]);
+ weight[0] = (1.0 - pchan->stiffness[0]);
+ weight[1] = (1.0 - pchan->stiffness[1]);
+ weight[2] = (1.0 - pchan->stiffness[2]);
switch (ikchan->jointType & ~IK_TRANSY) {
- case 0:
- // fixed bone
- if (!(ikchan->jointType & IK_TRANSY)) {
- joint += ":F";
- ret = arm->addSegment(joint, parent, KDL::Joint::None, 0.0, tip);
- }
- break;
- case IK_XDOF:
- // RX only, get the X rotation
- joint += ":RX";
- ret = arm->addSegment(joint, parent, KDL::Joint::RotX, rot[0], tip);
- weights.push_back(weight[0]);
- break;
- case IK_YDOF:
- // RY only, get the Y rotation
- joint += ":RY";
- ret = arm->addSegment(joint, parent, KDL::Joint::RotY, rot[0], tip);
- weights.push_back(weight[1]);
- break;
- case IK_ZDOF:
- // RZ only, get the Zz rotation
- joint += ":RZ";
- ret = arm->addSegment(joint, parent, KDL::Joint::RotZ, rot[0], tip);
- weights.push_back(weight[2]);
- break;
- case IK_XDOF|IK_YDOF:
- joint += ":RX";
- ret = arm->addSegment(joint, parent, KDL::Joint::RotX, rot[0]);
- weights.push_back(weight[0]);
- if (ret) {
- parent = joint;
- joint = bone->name;
- joint += ":RY";
- ret = arm->addSegment(joint, parent, KDL::Joint::RotY, rot[1], tip);
- weights.push_back(weight[1]);
- }
- break;
- case IK_SWING:
- joint += ":SW";
- ret = arm->addSegment(joint, parent, KDL::Joint::Swing, rot[0], tip);
- weights.push_back(weight[0]);
- weights.push_back(weight[2]);
- break;
- case IK_YDOF|IK_ZDOF:
- // RZ+RY
- joint += ":RZ";
- ret = arm->addSegment(joint, parent, KDL::Joint::RotZ, rot[0]);
- weights.push_back(weight[2]);
- if (ret) {
- parent = joint;
- joint = bone->name;
+ case 0:
+ // fixed bone
+ if (!(ikchan->jointType & IK_TRANSY)) {
+ joint += ":F";
+ ret = arm->addSegment(joint, parent, KDL::Joint::None, 0.0, tip);
+ }
+ break;
+ case IK_XDOF:
+ // RX only, get the X rotation
+ joint += ":RX";
+ ret = arm->addSegment(joint, parent, KDL::Joint::RotX, rot[0], tip);
+ weights.push_back(weight[0]);
+ break;
+ case IK_YDOF:
+ // RY only, get the Y rotation
joint += ":RY";
- ret = arm->addSegment(joint, parent, KDL::Joint::RotY, rot[1], tip);
+ ret = arm->addSegment(joint, parent, KDL::Joint::RotY, rot[0], tip);
weights.push_back(weight[1]);
- }
- break;
- case IK_SWING|IK_YDOF:
- // decompose in a Swing+RotY joint
- joint += ":SW";
- ret = arm->addSegment(joint, parent, KDL::Joint::Swing, rot[0]);
- weights.push_back(weight[0]);
- weights.push_back(weight[2]);
- if (ret) {
- parent = joint;
- joint = bone->name;
- joint += ":RY";
- ret = arm->addSegment(joint, parent, KDL::Joint::RotY, rot[2], tip);
+ break;
+ case IK_ZDOF:
+ // RZ only, get the Zz rotation
+ joint += ":RZ";
+ ret = arm->addSegment(joint, parent, KDL::Joint::RotZ, rot[0], tip);
+ weights.push_back(weight[2]);
+ break;
+ case IK_XDOF | IK_YDOF:
+ joint += ":RX";
+ ret = arm->addSegment(joint, parent, KDL::Joint::RotX, rot[0]);
+ weights.push_back(weight[0]);
+ if (ret) {
+ parent = joint;
+ joint = bone->name;
+ joint += ":RY";
+ ret = arm->addSegment(joint, parent, KDL::Joint::RotY, rot[1], tip);
+ weights.push_back(weight[1]);
+ }
+ break;
+ case IK_SWING:
+ joint += ":SW";
+ ret = arm->addSegment(joint, parent, KDL::Joint::Swing, rot[0], tip);
+ weights.push_back(weight[0]);
+ weights.push_back(weight[2]);
+ break;
+ case IK_YDOF | IK_ZDOF:
+ // RZ+RY
+ joint += ":RZ";
+ ret = arm->addSegment(joint, parent, KDL::Joint::RotZ, rot[0]);
+ weights.push_back(weight[2]);
+ if (ret) {
+ parent = joint;
+ joint = bone->name;
+ joint += ":RY";
+ ret = arm->addSegment(joint, parent, KDL::Joint::RotY, rot[1], tip);
+ weights.push_back(weight[1]);
+ }
+ break;
+ case IK_SWING | IK_YDOF:
+ // decompose in a Swing+RotY joint
+ joint += ":SW";
+ ret = arm->addSegment(joint, parent, KDL::Joint::Swing, rot[0]);
+ weights.push_back(weight[0]);
+ weights.push_back(weight[2]);
+ if (ret) {
+ parent = joint;
+ joint = bone->name;
+ joint += ":RY";
+ ret = arm->addSegment(joint, parent, KDL::Joint::RotY, rot[2], tip);
+ weights.push_back(weight[1]);
+ }
+ break;
+ case IK_REVOLUTE:
+ joint += ":SJ";
+ ret = arm->addSegment(joint, parent, KDL::Joint::Sphere, rot[0], tip);
+ weights.push_back(weight[0]);
weights.push_back(weight[1]);
- }
- break;
- case IK_REVOLUTE:
- joint += ":SJ";
- ret = arm->addSegment(joint, parent, KDL::Joint::Sphere, rot[0], tip);
- weights.push_back(weight[0]);
- weights.push_back(weight[1]);
- weights.push_back(weight[2]);
- break;
+ weights.push_back(weight[2]);
+ break;
}
if (ret && (ikchan->jointType & IK_TRANSY)) {
parent = joint;
joint = bone->name;
joint += ":TY";
- ret = arm->addSegment(joint, parent, KDL::Joint::TransY, rot[ikchan->ndof-1]);
- float ikstretch = pchan->ikstretch*pchan->ikstretch;
- weight[1] = (1.0-MIN2(1.0-ikstretch, 0.99));
+ ret = arm->addSegment(joint, parent, KDL::Joint::TransY, rot[ikchan->ndof - 1]);
+ float ikstretch = pchan->ikstretch * pchan->ikstretch;
+ weight[1] = (1.0 - MIN2(1.0 - ikstretch, 0.99));
weights.push_back(weight[1]);
}
if (!ret)
@@ -1269,7 +1266,7 @@ static IK_Scene* convert_tree(Scene *blscene, Object *ob, bPoseChannel *pchan)
ikchan->head = parent;
// in case of error
ret = false;
- if ((ikchan->jointType & IK_XDOF) && (pchan->ikflag & (BONE_IK_XLIMIT|BONE_IK_ROTCTL))) {
+ if ((ikchan->jointType & IK_XDOF) && (pchan->ikflag & (BONE_IK_XLIMIT | BONE_IK_ROTCTL))) {
joint = bone->name;
joint += ":RX";
if (pchan->ikflag & BONE_IK_XLIMIT) {
@@ -1281,7 +1278,7 @@ static IK_Scene* convert_tree(Scene *blscene, Object *ob, bPoseChannel *pchan)
break;
}
}
- if ((ikchan->jointType & IK_YDOF) && (pchan->ikflag & (BONE_IK_YLIMIT|BONE_IK_ROTCTL))) {
+ if ((ikchan->jointType & IK_YDOF) && (pchan->ikflag & (BONE_IK_YLIMIT | BONE_IK_ROTCTL))) {
joint = bone->name;
joint += ":RY";
if (pchan->ikflag & BONE_IK_YLIMIT) {
@@ -1293,7 +1290,7 @@ static IK_Scene* convert_tree(Scene *blscene, Object *ob, bPoseChannel *pchan)
break;
}
}
- if ((ikchan->jointType & IK_ZDOF) && (pchan->ikflag & (BONE_IK_ZLIMIT|BONE_IK_ROTCTL))) {
+ if ((ikchan->jointType & IK_ZDOF) && (pchan->ikflag & (BONE_IK_ZLIMIT | BONE_IK_ROTCTL))) {
joint = bone->name;
joint += ":RZ";
if (pchan->ikflag & BONE_IK_ZLIMIT) {
@@ -1305,7 +1302,7 @@ static IK_Scene* convert_tree(Scene *blscene, Object *ob, bPoseChannel *pchan)
break;
}
}
- if ((ikchan->jointType & IK_SWING) && (pchan->ikflag & (BONE_IK_XLIMIT|BONE_IK_ZLIMIT|BONE_IK_ROTCTL))) {
+ if ((ikchan->jointType & IK_SWING) && (pchan->ikflag & (BONE_IK_XLIMIT | BONE_IK_ZLIMIT | BONE_IK_ROTCTL))) {
joint = bone->name;
joint += ":SW";
if (pchan->ikflag & BONE_IK_XLIMIT) {
@@ -1336,13 +1333,13 @@ static IK_Scene* convert_tree(Scene *blscene, Object *ob, bPoseChannel *pchan)
return NULL;
}
// for each target, we need to add an end effector in the armature
- for (numtarget=0, polarcon=NULL, ret = true, target=(PoseTarget*)tree->targets.first; target; target=(PoseTarget*)target->next) {
- condata= (bKinematicConstraint*)target->con->data;
+ for (numtarget = 0, polarcon = NULL, ret = true, target = (PoseTarget *)tree->targets.first; target; target = (PoseTarget *)target->next) {
+ condata = (bKinematicConstraint *)target->con->data;
pchan = tree->pchan[target->tip];
if (is_cartesian_constraint(target->con)) {
// add the end effector
- IK_Target* iktarget = new IK_Target();
+ IK_Target *iktarget = new IK_Target();
ikscene->targets.push_back(iktarget);
iktarget->ee = arm->addEndEffector(ikscene->channels[target->tip].tail);
if (iktarget->ee == -1) {
@@ -1372,7 +1369,7 @@ static IK_Scene* convert_tree(Scene *blscene, Object *ob, bPoseChannel *pchan)
ikscene->polarConstraint = polarcon;
}
// we can now add the armature
- // the armature is based on a moving frame.
+ // the armature is based on a moving frame.
// initialize with the correct position in case there is no cache
base_callback(iTaSC::Timestamp(), iTaSC::F_identity, initPose, ikscene);
ikscene->base = new iTaSC::MovingFrame(initPose);
@@ -1392,8 +1389,8 @@ static IK_Scene* convert_tree(Scene *blscene, Object *ob, bPoseChannel *pchan)
// set the weight
e_matrix& Wq = arm->getWq();
assert(Wq.cols() == (int)weights.size());
- for (int q=0; q<Wq.cols(); q++)
- Wq(q, q)=weights[q];
+ 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
@@ -1401,9 +1398,9 @@ static IK_Scene* convert_tree(Scene *blscene, Object *ob, bPoseChannel *pchan)
float invBaseFrame[4][4];
pchan = ikscene->channels[0].pchan;
if (pchan->parent) {
- // it has a parent, get the pose matrix from it
+ // it has a parent, get the pose matrix from it
float baseFrame[4][4];
- pchan = pchan->parent;
+ pchan = pchan->parent;
copy_m4_m4(baseFrame, pchan->bone->arm_mat);
// move to the tail and scale to get rest pose of armature base
copy_v3_v3(baseFrame[3], pchan->bone->arm_tail);
@@ -1413,10 +1410,10 @@ static IK_Scene* convert_tree(Scene *blscene, Object *ob, bPoseChannel *pchan)
unit_m4(invBaseFrame);
}
// finally add the constraint
- for (t=0; t<ikscene->targets.size(); t++) {
- IK_Target* iktarget = ikscene->targets[t];
+ for (t = 0; t < ikscene->targets.size(); t++) {
+ IK_Target *iktarget = ikscene->targets[t];
iktarget->blscene = blscene;
- condata= (bKinematicConstraint*)iktarget->blenderConstraint->data;
+ condata = (bKinematicConstraint *)iktarget->blenderConstraint->data;
pchan = tree->pchan[iktarget->channel];
unsigned int controltype, bonecnt;
double bonelen;
@@ -1424,16 +1421,16 @@ static IK_Scene* convert_tree(Scene *blscene, Object *ob, bPoseChannel *pchan)
// add the end effector
// estimate the average bone length, used to clamp feedback error
- for (bonecnt=0, bonelen=0.f, a=iktarget->channel; a>=0; a=tree->parent[a], bonecnt++)
- bonelen += ikscene->blScale*tree->pchan[a]->bone->length;
- bonelen /= bonecnt;
+ for (bonecnt = 0, bonelen = 0.f, a = iktarget->channel; a >= 0; a = tree->parent[a], bonecnt++)
+ bonelen += ikscene->blScale * tree->pchan[a]->bone->length;
+ bonelen /= bonecnt;
// store the rest pose of the end effector to compute enforce target
copy_m4_m4(mat, pchan->bone->arm_mat);
copy_v3_v3(mat[3], pchan->bone->arm_tail);
// get the rest pose relative to the armature base
mult_m4_m4m4(iktarget->eeRest, invBaseFrame, mat);
- iktarget->eeBlend = (!ikscene->polarConstraint && condata->type==CONSTRAINT_IK_COPYPOSE) ? true : false;
+ iktarget->eeBlend = (!ikscene->polarConstraint && condata->type == CONSTRAINT_IK_COPYPOSE) ? true : false;
// use target_callback to make sure the initPose includes enforce coefficient
target_callback(iTaSC::Timestamp(), iTaSC::F_identity, initPose, iktarget);
iktarget->target = new iTaSC::MovingFrame(initPose);
@@ -1443,59 +1440,59 @@ static IK_Scene* convert_tree(Scene *blscene, Object *ob, bPoseChannel *pchan)
break;
switch (condata->type) {
- case CONSTRAINT_IK_COPYPOSE:
- controltype = 0;
- if (condata->flag & CONSTRAINT_IK_ROT) {
- if (!(condata->flag & CONSTRAINT_IK_NO_ROT_X))
- controltype |= iTaSC::CopyPose::CTL_ROTATIONX;
- if (!(condata->flag & CONSTRAINT_IK_NO_ROT_Y))
- controltype |= iTaSC::CopyPose::CTL_ROTATIONY;
- if (!(condata->flag & CONSTRAINT_IK_NO_ROT_Z))
- controltype |= iTaSC::CopyPose::CTL_ROTATIONZ;
- }
- if (condata->flag & CONSTRAINT_IK_POS) {
- if (!(condata->flag & CONSTRAINT_IK_NO_POS_X))
- controltype |= iTaSC::CopyPose::CTL_POSITIONX;
- if (!(condata->flag & CONSTRAINT_IK_NO_POS_Y))
- controltype |= iTaSC::CopyPose::CTL_POSITIONY;
- if (!(condata->flag & CONSTRAINT_IK_NO_POS_Z))
- controltype |= iTaSC::CopyPose::CTL_POSITIONZ;
- }
- if (controltype) {
- iktarget->constraint = new iTaSC::CopyPose(controltype, controltype, bonelen);
- // set the gain
- if (controltype & iTaSC::CopyPose::CTL_POSITION)
- iktarget->constraint->setControlParameter(iTaSC::CopyPose::ID_POSITION, iTaSC::ACT_ALPHA, condata->weight);
- if (controltype & iTaSC::CopyPose::CTL_ROTATION)
- iktarget->constraint->setControlParameter(iTaSC::CopyPose::ID_ROTATION, iTaSC::ACT_ALPHA, condata->orientweight);
- iktarget->constraint->registerCallback(copypose_callback, iktarget);
- iktarget->errorCallback = copypose_error;
- iktarget->controlType = controltype;
+ case CONSTRAINT_IK_COPYPOSE:
+ controltype = 0;
+ if (condata->flag & CONSTRAINT_IK_ROT) {
+ if (!(condata->flag & CONSTRAINT_IK_NO_ROT_X))
+ controltype |= iTaSC::CopyPose::CTL_ROTATIONX;
+ if (!(condata->flag & CONSTRAINT_IK_NO_ROT_Y))
+ controltype |= iTaSC::CopyPose::CTL_ROTATIONY;
+ if (!(condata->flag & CONSTRAINT_IK_NO_ROT_Z))
+ controltype |= iTaSC::CopyPose::CTL_ROTATIONZ;
+ }
+ if (condata->flag & CONSTRAINT_IK_POS) {
+ if (!(condata->flag & CONSTRAINT_IK_NO_POS_X))
+ controltype |= iTaSC::CopyPose::CTL_POSITIONX;
+ if (!(condata->flag & CONSTRAINT_IK_NO_POS_Y))
+ controltype |= iTaSC::CopyPose::CTL_POSITIONY;
+ if (!(condata->flag & CONSTRAINT_IK_NO_POS_Z))
+ controltype |= iTaSC::CopyPose::CTL_POSITIONZ;
+ }
+ if (controltype) {
+ iktarget->constraint = new iTaSC::CopyPose(controltype, controltype, bonelen);
+ // set the gain
+ if (controltype & iTaSC::CopyPose::CTL_POSITION)
+ iktarget->constraint->setControlParameter(iTaSC::CopyPose::ID_POSITION, iTaSC::ACT_ALPHA, condata->weight);
+ if (controltype & iTaSC::CopyPose::CTL_ROTATION)
+ iktarget->constraint->setControlParameter(iTaSC::CopyPose::ID_ROTATION, iTaSC::ACT_ALPHA, condata->orientweight);
+ iktarget->constraint->registerCallback(copypose_callback, iktarget);
+ iktarget->errorCallback = copypose_error;
+ iktarget->controlType = controltype;
+ // add the constraint
+ if (condata->flag & CONSTRAINT_IK_TARGETAXIS)
+ ret = scene->addConstraintSet(iktarget->constraintName, iktarget->constraint, iktarget->targetName, armname, "", ikscene->channels[iktarget->channel].tail);
+ else
+ ret = scene->addConstraintSet(iktarget->constraintName, iktarget->constraint, armname, iktarget->targetName, ikscene->channels[iktarget->channel].tail);
+ }
+ break;
+ case CONSTRAINT_IK_DISTANCE:
+ iktarget->constraint = new iTaSC::Distance(bonelen);
+ iktarget->constraint->setControlParameter(iTaSC::Distance::ID_DISTANCE, iTaSC::ACT_VALUE, condata->dist);
+ iktarget->constraint->registerCallback(distance_callback, iktarget);
+ iktarget->errorCallback = distance_error;
+ // we can update the weight on each substep
+ iktarget->constraint->substep(true);
// add the constraint
- if (condata->flag & CONSTRAINT_IK_TARGETAXIS)
- ret = scene->addConstraintSet(iktarget->constraintName, iktarget->constraint, iktarget->targetName, armname, "", ikscene->channels[iktarget->channel].tail);
- else
- ret = scene->addConstraintSet(iktarget->constraintName, iktarget->constraint, armname, iktarget->targetName, ikscene->channels[iktarget->channel].tail);
- }
- break;
- case CONSTRAINT_IK_DISTANCE:
- iktarget->constraint = new iTaSC::Distance(bonelen);
- iktarget->constraint->setControlParameter(iTaSC::Distance::ID_DISTANCE, iTaSC::ACT_VALUE, condata->dist);
- iktarget->constraint->registerCallback(distance_callback, iktarget);
- iktarget->errorCallback = distance_error;
- // we can update the weight on each substep
- iktarget->constraint->substep(true);
- // add the constraint
- ret = scene->addConstraintSet(iktarget->constraintName, iktarget->constraint, armname, iktarget->targetName, ikscene->channels[iktarget->channel].tail);
- break;
+ ret = scene->addConstraintSet(iktarget->constraintName, iktarget->constraint, armname, iktarget->targetName, ikscene->channels[iktarget->channel].tail);
+ break;
}
if (!ret)
break;
}
if (!ret ||
- !scene->addCache(ikscene->cache) ||
- !scene->addSolver(ikscene->solver) ||
- !scene->initialize()) {
+ !scene->addCache(ikscene->cache) ||
+ !scene->addSolver(ikscene->solver) ||
+ !scene->initialize()) {
delete ikscene;
ikscene = NULL;
}
@@ -1507,13 +1504,13 @@ static void create_scene(Scene *scene, Object *ob)
bPoseChannel *pchan;
// create the IK scene
- for (pchan= (bPoseChannel *)ob->pose->chanbase.first; pchan; pchan= (bPoseChannel *)pchan->next) {
+ for (pchan = (bPoseChannel *)ob->pose->chanbase.first; pchan; pchan = (bPoseChannel *)pchan->next) {
// by construction there is only one tree
- PoseTree *tree = (PoseTree*)pchan->iktree.first;
+ PoseTree *tree = (PoseTree *)pchan->iktree.first;
if (tree) {
- IK_Data* ikdata = get_ikdata(ob->pose);
+ IK_Data *ikdata = get_ikdata(ob->pose);
// convert tree in iTaSC::Scene
- IK_Scene* ikscene = convert_tree(scene, ob, pchan);
+ IK_Scene *ikscene = convert_tree(scene, ob, pchan);
if (ikscene) {
ikscene->next = ikdata->first;
ikdata->first = ikscene;
@@ -1526,7 +1523,7 @@ static void create_scene(Scene *scene, Object *ob)
if (tree->parent) MEM_freeN(tree->parent);
if (tree->basis_change) MEM_freeN(tree->basis_change);
MEM_freeN(tree);
- tree = (PoseTree*)pchan->iktree.first;
+ tree = (PoseTree *)pchan->iktree.first;
}
}
}
@@ -1537,12 +1534,12 @@ static int init_scene(Object *ob)
{
// check also if scaling has changed
float scale = len_v3(ob->obmat[1]);
- IK_Scene* scene;
+ IK_Scene *scene;
if (ob->pose->ikdata) {
- for (scene = ((IK_Data*)ob->pose->ikdata)->first;
- scene != NULL;
- scene = scene->next) {
+ for (scene = ((IK_Data *)ob->pose->ikdata)->first;
+ scene != NULL;
+ scene = scene->next) {
if (fabs(scene->blScale - scale) > KDL::epsilon)
return 1;
scene->channels[0].pchan->flag |= POSE_IKTREE;
@@ -1551,31 +1548,31 @@ static int init_scene(Object *ob)
return 0;
}
-static void execute_scene(Scene* blscene, IK_Scene* ikscene, bItasc* ikparam, float ctime, float frtime)
+static void execute_scene(Scene *blscene, IK_Scene *ikscene, bItasc *ikparam, float ctime, float frtime)
{
int i;
- IK_Channel* ikchan;
+ IK_Channel *ikchan;
if (ikparam->flag & ITASC_SIMULATION) {
- for (i=0, ikchan=ikscene->channels; i<ikscene->numchan; i++, ++ikchan) {
+ for (i = 0, ikchan = ikscene->channels; i < ikscene->numchan; i++, ++ikchan) {
// In simulation mode we don't allow external contraint to change our bones, mark the channel done
// also tell Blender that this channel is part of IK tree (cleared on each BKE_pose_where_is()
- ikchan->pchan->flag |= (POSE_DONE|POSE_CHAIN);
+ ikchan->pchan->flag |= (POSE_DONE | POSE_CHAIN);
ikchan->jointValid = 0;
}
}
else {
// in animation mode, we must get the bone position from action and constraints
- for (i=0, ikchan=ikscene->channels; i<ikscene->numchan; i++, ++ikchan) {
+ for (i = 0, ikchan = ikscene->channels; i < ikscene->numchan; i++, ++ikchan) {
if (!(ikchan->pchan->flag & POSE_DONE))
BKE_pose_where_is_bone(blscene, ikscene->blArmature, ikchan->pchan, ctime, 1);
// tell blender that this channel was controlled by IK, it's cleared on each BKE_pose_where_is()
- ikchan->pchan->flag |= (POSE_DONE|POSE_CHAIN);
+ ikchan->pchan->flag |= (POSE_DONE | POSE_CHAIN);
ikchan->jointValid = 0;
}
}
// only run execute the scene if at least one of our target is enabled
- for (i=ikscene->targets.size(); i > 0; --i) {
- IK_Target* iktarget = ikscene->targets[i-1];
+ for (i = ikscene->targets.size(); i > 0; --i) {
+ IK_Target *iktarget = ikscene->targets[i - 1];
if (!(iktarget->blenderConstraint->flag & CONSTRAINT_OFF))
break;
}
@@ -1592,7 +1589,7 @@ static void execute_scene(Scene* blscene, IK_Scene* ikscene, bItasc* ikparam, fl
if (ikparam->flag & ITASC_SIMULATION) {
ikscene->solver->setParam(iTaSC::Solver::DLS_QMAX, ikparam->maxvel);
- }
+ }
else {
// in animation mode we start from the pose after action and constraint
convert_pose(ikscene);
@@ -1601,15 +1598,15 @@ static void execute_scene(Scene* blscene, IK_Scene* ikscene, bItasc* ikparam, fl
reiterate = true;
simulation = false;
// time is virtual, so take fixed value for velocity parameters (see itasc_update_param)
- // and choose 1s timestep to allow having velocity parameters in radiant
+ // and choose 1s timestep to allow having velocity parameters in radiant
timestep = 1.0;
// use auto setup to let the solver test the variation of the joints
numstep = 0;
}
-
+
if (ikscene->cache && !reiterate && simulation) {
iTaSC::CacheTS sts, cts;
- sts = cts = (iTaSC::CacheTS)(timestamp*1000.0+0.5);
+ sts = cts = (iTaSC::CacheTS)(timestamp * 1000.0 + 0.5);
if (ikscene->cache->getPreviousCacheItem(ikscene->armature, 0, &cts) == NULL || cts == 0) {
// the cache is empty before this time, reiterate
if (ikparam->flag & ITASC_INITIAL_REITERATION)
@@ -1618,14 +1615,14 @@ static void execute_scene(Scene* blscene, IK_Scene* ikscene, bItasc* ikparam, fl
else {
// can take the cache as a start point.
sts -= cts;
- timestep = sts/1000.0;
+ timestep = sts / 1000.0;
}
}
// don't cache if we are reiterating because we don't want to distroy the cache unnecessarily
ikscene->scene->update(timestamp, timestep, numstep, false, !reiterate, simulation);
if (reiterate) {
// how many times do we reiterate?
- for (i = 0; i<ikparam->numiter; i++) {
+ for (i = 0; i < ikparam->numiter; i++) {
if (ikscene->armature->getMaxJointChange() < ikparam->precision ||
ikscene->armature->getMaxEndEffectorChange() < ikparam->precision)
{
@@ -1639,11 +1636,11 @@ static void execute_scene(Scene* blscene, IK_Scene* ikscene, bItasc* ikparam, fl
}
}
// compute constraint error
- for (i=ikscene->targets.size(); i > 0; --i) {
- IK_Target* iktarget = ikscene->targets[i-1];
+ for (i = ikscene->targets.size(); i > 0; --i) {
+ IK_Target *iktarget = ikscene->targets[i - 1];
if (!(iktarget->blenderConstraint->flag & CONSTRAINT_OFF)) {
unsigned int nvalues;
- const iTaSC::ConstraintValues* values;
+ const iTaSC::ConstraintValues *values;
values = iktarget->constraint->getControlParameters(&nvalues);
iktarget->errorCallback(values, nvalues, iktarget);
}
@@ -1654,22 +1651,22 @@ static void execute_scene(Scene* blscene, IK_Scene* ikscene, bItasc* ikparam, fl
// combine the parent and the joint frame to get the frame relative to armature
// a backward translation of the bone length gives the head
// if TY, compute the scale as the ratio of the joint length with rest pose length
- iTaSC::Armature* arm = ikscene->armature;
+ iTaSC::Armature *arm = ikscene->armature;
KDL::Frame frame;
double q_rest[3], q[3];
- const KDL::Joint* joint;
- const KDL::Frame* tip;
- bPoseChannel* pchan;
+ const KDL::Joint *joint;
+ const KDL::Frame *tip;
+ bPoseChannel *pchan;
float scale;
float length;
float yaxis[3];
- for (i=0, ikchan=ikscene->channels; i<ikscene->numchan; ++i, ++ikchan) {
+ for (i = 0, ikchan = ikscene->channels; i < ikscene->numchan; ++i, ++ikchan) {
if (i == 0) {
if (!arm->getRelativeFrame(frame, ikchan->tail))
break;
// this frame is relative to base, make it relative to object
ikchan->frame = ikscene->baseFrame * frame;
- }
+ }
else {
if (!arm->getRelativeFrame(frame, ikchan->tail, ikscene->channels[ikchan->parent].tail))
break;
@@ -1682,10 +1679,10 @@ static void execute_scene(Scene* blscene, IK_Scene* ikscene, bItasc* ikparam, fl
break;
if (joint->getType() == KDL::Joint::TransY) {
// stretch bones have a TY joint, compute the scale
- scale = (float)(q[0]/q_rest[0]);
+ scale = (float)(q[0] / q_rest[0]);
// the length is the joint itself
length = (float)q[0];
- }
+ }
else {
scale = 1.0f;
// for fixed bone, the length is in the tip (always along Y axis)
@@ -1710,7 +1707,7 @@ static void execute_scene(Scene* blscene, IK_Scene* ikscene, bItasc* ikparam, fl
mul_v3_fl(pchan->pose_mat[1], scale);
mul_v3_fl(pchan->pose_mat[2], scale);
}
- if (i<ikscene->numchan) {
+ if (i < ikscene->numchan) {
// big problem
;
}
@@ -1732,11 +1729,11 @@ void itasc_initialize_tree(struct Scene *scene, Object *ob, float ctime)
itasc_clear_data(ob->pose);
// we should handle all the constraint and mark them all disabled
// for blender but we'll start with the IK constraint alone
- for (pchan= (bPoseChannel *)ob->pose->chanbase.first; pchan; pchan= (bPoseChannel *)pchan->next) {
+ for (pchan = (bPoseChannel *)ob->pose->chanbase.first; pchan; pchan = (bPoseChannel *)pchan->next) {
if (pchan->constflag & PCHAN_HAS_IK)
count += initialize_scene(ob, pchan);
}
- // if at least one tree, create the scenes from the PoseTree stored in the channels
+ // if at least one tree, create the scenes from the PoseTree stored in the channels
if (count)
create_scene(scene, ob);
itasc_update_param(ob->pose);
@@ -1747,14 +1744,14 @@ void itasc_initialize_tree(struct Scene *scene, Object *ob, float ctime)
void itasc_execute_tree(struct Scene *scene, struct Object *ob, struct bPoseChannel *pchan, float ctime)
{
if (ob->pose->ikdata) {
- IK_Data* ikdata = (IK_Data*)ob->pose->ikdata;
- bItasc* ikparam = (bItasc*) ob->pose->ikparam;
+ IK_Data *ikdata = (IK_Data *)ob->pose->ikdata;
+ bItasc *ikparam = (bItasc *) ob->pose->ikparam;
// we need default parameters
if (!ikparam) ikparam = &DefIKParam;
- for (IK_Scene* ikscene = ikdata->first; ikscene; ikscene = ikscene->next) {
+ for (IK_Scene *ikscene = ikdata->first; ikscene; ikscene = ikscene->next) {
if (ikscene->channels[0].pchan == pchan) {
- float timestep = scene->r.frs_sec_base/scene->r.frs_sec;
+ float timestep = scene->r.frs_sec_base / scene->r.frs_sec;
if (ob->pose->flag & POSE_GAME_ENGINE) {
timestep = ob->pose->ctime;
// limit the timestep to avoid excessive number of iteration
@@ -1776,8 +1773,8 @@ void itasc_release_tree(struct Scene *scene, struct Object *ob, float ctime)
void itasc_clear_data(struct bPose *pose)
{
if (pose->ikdata) {
- IK_Data* ikdata = (IK_Data*)pose->ikdata;
- for (IK_Scene* scene = ikdata->first; scene; scene = ikdata->first) {
+ IK_Data *ikdata = (IK_Data *)pose->ikdata;
+ for (IK_Scene *scene = ikdata->first; scene; scene = ikdata->first) {
ikdata->first = scene->next;
delete scene;
}
@@ -1789,8 +1786,8 @@ void itasc_clear_data(struct bPose *pose)
void itasc_clear_cache(struct bPose *pose)
{
if (pose->ikdata) {
- IK_Data* ikdata = (IK_Data*)pose->ikdata;
- for (IK_Scene* scene = ikdata->first; scene; scene = scene->next) {
+ IK_Data *ikdata = (IK_Data *)pose->ikdata;
+ for (IK_Scene *scene = ikdata->first; scene; scene = scene->next) {
if (scene->cache)
// clear all cache but leaving the timestamp 0 (=rest pose)
scene->cache->clearCacheFrom(NULL, 1);
@@ -1801,12 +1798,12 @@ void itasc_clear_cache(struct bPose *pose)
void itasc_update_param(struct bPose *pose)
{
if (pose->ikdata && pose->ikparam) {
- IK_Data* ikdata = (IK_Data*)pose->ikdata;
- bItasc* ikparam = (bItasc*)pose->ikparam;
- for (IK_Scene* ikscene = ikdata->first; ikscene; ikscene = ikscene->next) {
+ IK_Data *ikdata = (IK_Data *)pose->ikdata;
+ bItasc *ikparam = (bItasc *)pose->ikparam;
+ for (IK_Scene *ikscene = ikdata->first; ikscene; ikscene = ikscene->next) {
double armlength = ikscene->armature->getArmLength();
- ikscene->solver->setParam(iTaSC::Solver::DLS_LAMBDA_MAX, ikparam->dampmax*armlength);
- ikscene->solver->setParam(iTaSC::Solver::DLS_EPSILON, ikparam->dampeps*armlength);
+ ikscene->solver->setParam(iTaSC::Solver::DLS_LAMBDA_MAX, ikparam->dampmax * armlength);
+ ikscene->solver->setParam(iTaSC::Solver::DLS_EPSILON, ikparam->dampeps * armlength);
if (ikparam->flag & ITASC_SIMULATION) {
ikscene->scene->setParam(iTaSC::Scene::MIN_TIMESTEP, ikparam->minstep);
ikscene->scene->setParam(iTaSC::Scene::MAX_TIMESTEP, ikparam->maxstep);
@@ -1814,7 +1811,7 @@ void itasc_update_param(struct bPose *pose)
ikscene->armature->setControlParameter(CONSTRAINT_ID_ALL, iTaSC::Armature::ID_JOINT, iTaSC::ACT_FEEDBACK, ikparam->feedback);
}
else {
- // in animation mode timestep is 1s by convention =>
+ // in animation mode timestep is 1s by convention =>
// qmax becomes radiant and feedback becomes fraction of error gap corrected in one iteration
ikscene->scene->setParam(iTaSC::Scene::MIN_TIMESTEP, 1.0);
ikscene->scene->setParam(iTaSC::Scene::MAX_TIMESTEP, 1.0);
@@ -1834,10 +1831,10 @@ void itasc_test_constraint(struct Object *ob, struct bConstraint *cons)
return;
switch (data->type) {
- case CONSTRAINT_IK_COPYPOSE:
- case CONSTRAINT_IK_DISTANCE:
- /* cartesian space constraint */
- break;
+ case CONSTRAINT_IK_COPYPOSE:
+ case CONSTRAINT_IK_DISTANCE:
+ /* cartesian space constraint */
+ break;
}
}
generated by cgit v1.2.3 (git 2.25.1) at 2024-07-09 08:31:22 +0300