[Bf-blender-cvs] SVN commit: /data/svn/bf-blender [22970] branches/itasc: iTaSC: Support enforce parameter.
Benoit Bolsee
benoit.bolsee at online.be
Thu Sep 3 18:24:39 CEST 2009
Revision: 22970
http://projects.blender.org/plugins/scmsvn/viewcvs.php?view=rev&root=bf-blender&revision=22970
Author: ben2610
Date: 2009-09-03 18:24:25 +0200 (Thu, 03 Sep 2009)
Log Message:
-----------
iTaSC: Support enforce parameter.
The implementation is similar to old solver:
the target position is blended with the position
that the end effector would take if the armature
was in rest pose. Enforce is not supported if polar
target is used: the rotation of the armature would
interfere with the blending and post blending of
pose, as it's done in the old solver, doesn't make
sense in a stateful solver.
Modified Paths:
--------------
branches/itasc/intern/itasc/Armature.cpp
branches/itasc/source/blender/ikplugin/intern/itasc_plugin.cpp
Modified: branches/itasc/intern/itasc/Armature.cpp
===================================================================
--- branches/itasc/intern/itasc/Armature.cpp 2009-09-03 12:20:59 UTC (rev 22969)
+++ branches/itasc/intern/itasc/Armature.cpp 2009-09-03 16:24:25 UTC (rev 22970)
@@ -540,7 +540,7 @@
}
case KDL::Joint::Sphere:
{
- (KDL::Rot(KDL::Vector(q))*KDL::Rot(KDL::Vector(qdot)*timestamp.realTimestep)).GetRot().GetValue(q);
+ (KDL::Rot(KDL::Vector(q))*KDL::Rot(KDL::Vector(qdot)*timestamp.realTimestep)).GetRot().GetValue(newq);
// no limit on this joint
unlocked++;
break;
Modified: branches/itasc/source/blender/ikplugin/intern/itasc_plugin.cpp
===================================================================
--- branches/itasc/source/blender/ikplugin/intern/itasc_plugin.cpp 2009-09-03 12:20:59 UTC (rev 22969)
+++ branches/itasc/source/blender/ikplugin/intern/itasc_plugin.cpp 2009-09-03 16:24:25 UTC (rev 22970)
@@ -83,20 +83,24 @@
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 int controlType;
- int ee; //end effector number
- KDL::Frame eeRest; //end effector initial pose relative to armature
+ short ee; //end effector number
+ bool eeBlend; //end effector affected by enforce blending
+ float eeRest[4][4]; //end effector initial pose relative to armature
IK_Target() {
target = NULL;
constraint = NULL;
blenderConstraint = NULL;
+ rootChannel = NULL;
owner = NULL;
ee = 0;
+ eeBlend = true;
targetName.reserve(32);
constraintName.reserve(32);
}
@@ -332,7 +336,7 @@
}
if (data->tar==NULL) continue;
if (data->tar->type==OB_ARMATURE && data->subtarget[0]==0) continue;
- if ((con->flag & CONSTRAINT_DISABLE)==0 && (con->enforce!=0.0)) {
+ if ((con->flag & CONSTRAINT_DISABLE)==0) {
treecount += initialize_chain(ob, pchan_tip, con);
continue;
}
@@ -431,16 +435,33 @@
IK_Target* target = (IK_Target*)param;
// compute next target position
// get target matrix from constraint.
- bKinematicConstraint* data;
- float rootmat[4][4];
+ bConstraint* constraint = (bConstraint*)target->blenderConstraint;
+ float tarmat[4][4];
- data = (bKinematicConstraint*)target->blenderConstraint->data;
+ get_constraint_target_matrix(constraint, 0, CONSTRAINT_OBTYPE_OBJECT, target->owner, tarmat, 1.0);
- get_constraint_target_matrix(target->blenderConstraint, 0, CONSTRAINT_OBTYPE_OBJECT, target->owner, rootmat, 1.0);
-
- // rootmat contains the target pose in world coordinate, just put in next pose
- // TBD: blending, pole target
- next.setValue(&rootmat[0][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];
+ Mat4CpyMat4(chanmat, pchan->pose_mat);
+ VECCOPY(chanmat[3], pchan->pose_tail);
+ Mat4MulSerie(restmat, target->owner->obmat, chanmat, target->eeRest, NULL, NULL, NULL, NULL, NULL);
+ }
+ else {
+ Mat4MulMat4(restmat, target->eeRest, target->owner->obmat);
+ }
+ // blend the target
+ Mat4BlendMat4(tarmat, restmat, tarmat, constraint->enforce);
+ }
+ next.setValue(&tarmat[0][0]);
return true;
}
@@ -990,8 +1011,6 @@
}
// 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) {
- float rootmat[4][4];
-
condata= (bKinematicConstraint*)target->con->data;
pchan = tree->pchan[target->tip];
@@ -1003,18 +1022,6 @@
ret = false;
break;
}
- // same for target, we don't know the rest position make it world
- get_constraint_target_matrix(target->con, 0, CONSTRAINT_OBTYPE_OBJECT, ob, rootmat, 1.0);
- initPose.setValue(&rootmat[0][0]);
- iktarget->target = new iTaSC::MovingFrame(initPose);
- iktarget->target->setCallback(target_callback, iktarget);
- iktarget->targetName = pchan->bone->name;
- iktarget->targetName += ":T:";
- iktarget->targetName += target->con->name;
- if (!scene->addObject(iktarget->targetName, iktarget->target)) {
- ret = false;
- break;
- }
numtarget++;
if (condata->poletar)
// this constraint has a polar target
@@ -1028,7 +1035,7 @@
// 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;
+ ikscene->base = new iTaSC::MovingFrame(initPose);
ikscene->base->setCallback(base_callback, ikscene);
std::string armname;
armname = ob->id.name;
@@ -1051,20 +1058,26 @@
// 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
- KDL::Frame invBaseFrame;
+ float invBaseFrame[4][4];
pchan = ikscene->channels[0].pchan;
if (pchan->parent) {
- // it has a parent, get the pose matrix from it and move to the tail
+ // it has a parent, get the pose matrix from it
+ float baseFrame[4][4];
pchan = pchan->parent;
- invBaseFrame.setValue(&pchan->bone->arm_mat[0][0]);
- invBaseFrame = invBaseFrame.Inverse();
- }
+ Mat4CpyMat4(baseFrame, pchan->bone->arm_mat);
+ // move to the tail and scale to get rest pose of armature base
+ VecCopyf(baseFrame[3], pchan->bone->arm_tail);
+ Mat4Invert(invBaseFrame, baseFrame);
+ } else {
+ Mat4One(invBaseFrame);
+ }
// finally add the constraint
for (t=0, target=(PoseTarget*)tree->targets.first; target; target=(PoseTarget*)target->next, t++) {
condata= (bKinematicConstraint*)target->con->data;
pchan = tree->pchan[target->tip];
unsigned int controltype, bonecnt;
double bonelen;
+ float mat[4][4];
// add the end effector
IK_Target* iktarget = ikscene->targets[t];
@@ -1073,6 +1086,29 @@
bonelen += scale*tree->pchan[a]->bone->length;
bonelen /= bonecnt;
+ // store the rest pose of the end effector to compute enforce target
+ Mat4CpyMat4(mat, pchan->bone->arm_mat);
+ VecCopyf(mat[3], pchan->bone->arm_tail);
+ // get the rest pose relative to the armature base
+ Mat4MulMat4(iktarget->eeRest, mat, invBaseFrame);
+ // initialize iktarget
+ iktarget->rootChannel = ikscene->channels[0].pchan;
+ iktarget->eeBlend = (!ikscene->polarConstraint && condata->type==CONSTRAINT_IK_COPYPOSE) ? true : false;
+ iktarget->owner = ob;
+ iktarget->blenderConstraint = target->con;
+ iktarget->targetName = pchan->bone->name;
+ iktarget->targetName += ":T:";
+ iktarget->targetName += target->con->name;
+ // the above fields must be set before calling target_callback
+ // 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);
+ if (!scene->addObject(iktarget->targetName, iktarget->target)) {
+ ret = false;
+ break;
+ }
+
switch (condata->type) {
case CONSTRAINT_IK_COPYPOSE:
controltype = 0;
@@ -1082,7 +1118,6 @@
controltype |= iTaSC::CopyPose::CTL_POSITION;
if (controltype) {
iktarget->constraint = new iTaSC::CopyPose(controltype, controltype, bonelen);
- iktarget->blenderConstraint = target->con;
// set the gain
if (controltype & iTaSC::CopyPose::CTL_POSITION)
iktarget->constraint->setControlParameter(iTaSC::CopyPose::ID_POSITION, iTaSC::ACT_ALPHA, condata->weight);
@@ -1100,7 +1135,6 @@
break;
case CONSTRAINT_IK_DISTANCE:
iktarget->constraint = new iTaSC::Distance(bonelen);
- iktarget->blenderConstraint = target->con;
iktarget->constraint->setControlParameter(iTaSC::Distance::ID_DISTANCE, iTaSC::ACT_VALUE, condata->dist);
iktarget->constraint->registerCallback(distance_callback, iktarget);
iktarget->errorCallback = distance_error;
@@ -1115,12 +1149,6 @@
}
if (!ret)
break;
-
- // store the rest pose of the end effector to compute enforce target
- iktarget->eeRest.setValue(&pchan->bone->arm_mat[0][0]);
- // must remove the frame of the armature base
- iktarget->eeRest = invBaseFrame * iktarget->eeRest;
- iktarget->owner = ob;
}
if (!ret ||
!scene->addCache(ikscene->cache) ||
More information about the Bf-blender-cvs
mailing list