diff options
Diffstat (limited to 'source/blender/ikplugin/intern/itasc_plugin.cpp')
| -rw-r--r-- | source/blender/ikplugin/intern/itasc_plugin.cpp | 1786 |
1 files changed, 1786 insertions, 0 deletions
diff --git a/source/blender/ikplugin/intern/itasc_plugin.cpp b/source/blender/ikplugin/intern/itasc_plugin.cpp new file mode 100644 index 00000000000..b6278e40ea5 --- /dev/null +++ b/source/blender/ikplugin/intern/itasc_plugin.cpp @@ -0,0 +1,1786 @@ +/** + * $Id$ + * ***** BEGIN GPL LICENSE BLOCK ***** + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software Foundation, + * Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV. + * All rights reserved. + * + * The Original Code is: all of this file. + * + * Original author: Benoit Bolsee + * Contributor(s): + * + * ***** END GPL LICENSE BLOCK ***** + */ + +#include <stdlib.h> +#include <string.h> +#include <vector> + +// iTaSC headers +#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 "MEM_guardedalloc.h" + +extern "C" { +#include "BIK_api.h" +#include "BLI_blenlib.h" +#include "BLI_arithb.h" + +#include "BKE_global.h" +#include "BKE_armature.h" +#include "BKE_action.h" +#include "BKE_utildefines.h" +#include "BKE_constraint.h" +#include "DNA_object_types.h" +#include "DNA_action_types.h" +#include "DNA_constraint_types.h" +#include "DNA_armature_types.h" +#include "DNA_scene_types.h" +}; + +#include "itasc_plugin.h" + +// default parameters +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 + + +// 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; +}; + +typedef float Vector3[3]; +typedef float Vector4[4]; +struct IK_Target; +typedef void (*ErrorCallback)(const iTaSC::ConstraintValues* values, unsigned int nvalues, IK_Target* iktarget); +// For some reason, gcc doesn't find the declaration of this function in linux +void KDL::SetToZero(JntArray& array); + +// one structure for each target in the scene +struct IK_Target +{ + 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() { + 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; + } +}; + +struct IK_Scene +{ + 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; + struct bConstraint* polarConstraint; + std::vector<IK_Target*> targets; + + IK_Scene() { + next = NULL; + channels = NULL; + armature = NULL; + cache = NULL; + scene = NULL; + base = NULL; + solver = NULL; + 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, +}; + +enum IK_SegmentAxis { + 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) && curchan->iktree.first == NULL){ + rootbone++; + continue; + } + break; + } + + if (curchan->iktree.first != NULL) + // 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 begining 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) && 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; + 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++); + } + parent= a-1; + segcount= segcount-a; + target->tip= tree->totchannel + segcount - 1; + + if (segcount > 0) { + /* 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; + + 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) + 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; +} + +int initialize_scene(Object *ob, bPoseChannel *pchan_tip) +{ + bPoseChannel *curchan, *pchan_root=NULL, *chanlist[256], **oldchan; + PoseTree *tree; + PoseTarget *target; + bConstraint *con; + bKinematicConstraint *data; + int a, segcount= 0, size, newsize, *oldparent, parent, rootbone, 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) + init_pose_itasc(&DefIKParam); + + 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)); + + 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) +{ + // 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); + + return tau; +} + +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; +} + +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)); + } +} + +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)); + } +} + +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; + } +} + +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]; + + get_constraint_target_matrix(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]; + 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; +} + +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]; + Mat4CpyMat4(chanmat, pchan->pose_mat); + VECCOPY(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]); + Mat4MulMat4(rootmat, chanmat, ikscene->blArmature->obmat); + } + else { + Mat4CpyMat4(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; + + Mat4Invert(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 + get_constraint_target_matrix(ikscene->polarConstraint, 1, CONSTRAINT_OBTYPE_OBJECT, ikscene->blArmature, mat, 1.0); + // convert to armature space + Mat4MulMat4(polemat, mat, imat); + // 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 + Mat4MulMat4(goalmat, mat, imat); + // 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) +{ + IK_Target* iktarget =(IK_Target*)_param; + bKinematicConstraint *condata = (bKinematicConstraint *)iktarget->blenderConstraint->data; + iTaSC::ConstraintValues* values = _values; + bItasc* ikparam = (bItasc*) iktarget->owner->pose->ikparam; + iTaSC::ConstraintSingleValue* value; + double error; + int i; + + // 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) +{ + 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) +{ + 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) +{ + 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) +{ + 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) */ + EulOToMat3(chan->eul, chan->rotmode, rmat); + } + else if (chan->rotmode == PCHAN_ROT_AXISANGLE) { + /* axis-angle - stored in quaternion data, but not really that great for 3D-changing orientations */ + AxisAngleToMat3(&chan->quat[1], chan->quat[0], rmat); + } + else { + /* quats are normalised before use to eliminate scaling issues */ + NormalQuat(chan->quat); + QuatToMat3(chan->quat, rmat); + } + 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 (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(IK_Scene *ikscene, PoseTree *tree) +{ + IK_Channel *ikchan; + bPoseChannel *pchan; + PoseTarget* target; + Bone *bone; + int a, flag, njoint; + + njoint = 0; + for(a=0, ikchan = ikscene->channels; a<ikscene->numchan; ++a, ++ikchan) { + pchan= tree->pchan[a]; + bone= pchan->bone; + ikchan->pchan = pchan; + 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)) + 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 = VecLength(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) { + Mat4One(bmat); + Mat4MulMat43(bmat, pchan->parent->pose_mat, bone->bone_mat); + } else { + Mat4CpyMat4(bmat, bone->arm_mat); + } + Mat4Invert(rmat, bmat); + Mat4MulMat4(bmat, pchan->pose_mat, rmat); + Mat4Ortho(bmat); + boneRot.setValue(bmat[0]); + GetJointRotation(boneRot, ikchan->jointType, rot); + if (ikchan->jointType & IK_TRANSY) { + // compute actual length + rot[ikchan->ndof-1] = VecLenf(pchan->pose_tail, pchan->pose_head) * scale; + } + rot += ikchan->ndof; + joint += ikchan->ndof; + } +} + +// compute array of joint value corresponding to current pose +static void rest_pose(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 = VecLength(ikscene->blArmature->obmat[1]); + // rest pose is 0 + KDL::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(Scene *blscene, Object *ob, bPoseChannel *pchan) +{ + 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; + KDL::Rotation boneRot; + Bone *bone; + int a, t, numtarget; + float length; + bool ret = true, ingame; + double *rot; + double lmin[3], lmax[3]; + + if (tree->totchannel == 0) + return NULL; + + ikscene = new IK_Scene; + 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; + ingame = (ob->pose->flag & POSE_GAME_ENGINE); + if (!ikparam) { + // you must have our own copy + ikparam = &DefIKParam; + } else if (ingame) { + // tweak the param when in game to have efficient stepping + // using fixed substep is not effecient since frames in the GE are often + // 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; + 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 + ikparam->minstep = ikparam->maxstep = timestep; + ikparam->flag |= ITASC_AUTO_STEP; + } + } + if ((ikparam->flag & ITASC_SIMULATION) && !ingame) + // 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; + + std::string joint; + std::string root("root"); + std::string parent; + std::vector<double> weights; + double weight[3]; + // assume uniform scaling and take Y scale as general scale for the armature + float scale = VecLength(ob->obmat[1]); + double X, Y, Z; + // build the array of joints corresponding to the IK chain + convert_channels(ikscene, tree); + if (ingame) { + // in the GE, set the initial joint angle to match the current pose + // this will update the jointArray in ikscene + convert_pose(ikscene); + } else { + // in Blender, the rest pose is always 0 for joints + rest_pose(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); + Vector3 *fl = bone->bone_mat; + KDL::Frame head(KDL::Rotation( + 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(bone->head[0], bone->head[1], bone->head[2])*scale); + + // rest pose length of the bone taking scaling into account + length= bone->length*scale; + 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) { + 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 + 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; + 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]); + float ikstretch = pchan->ikstretch*pchan->ikstretch; + weight[1] = (1.0-MIN2(1.0-ikstretch, 0.99)); + 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() == weights.size()); + for (unsigned 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; + 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; t<ikscene->targets.size(); t++) { + IK_Target* iktarget = ikscene->targets[t]; + 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 += 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); + 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) && (condata->orientweight != 0.0)) + controltype |= iTaSC::CopyPose::CTL_ROTATION; + if ((condata->weight != 0.0)) + controltype |= iTaSC::CopyPose::CTL_POSITION; + 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 + 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(Scene *scene, Object *ob) +{ + 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(scene, ob, pchan); + 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; + } + } + } +} + +static void init_scene(Object *ob) +{ + bPoseChannel *pchan; + + if (ob->pose->ikdata) { + for(IK_Scene* scene = ((IK_Data*)ob->pose->ikdata)->first; + scene != NULL; + scene = scene->next) { + scene->channels[0].pchan->flag |= POSE_IKTREE; + } + } +} + +static void execute_scene(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 where_is_pose() + 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)) + where_is_pose_bone(blscene, ikscene->blArmature, ikchan->pchan, ctime); + // tell blender that this channel was controlled by IK, it's cleared on each where_is_pose() + 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, dts; + 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 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++) { + 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)) { + 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]); + VECCOPY(pchan->pose_tail, pchan->pose_mat[3]); + // shift to head + VECCOPY(yaxis, pchan->pose_mat[1]); + VecMulf(yaxis, length); + VecSubf(pchan->pose_mat[3], pchan->pose_mat[3], yaxis); + VECCOPY(pchan->pose_head, pchan->pose_mat[3]); + // add scale + VecMulf(pchan->pose_mat[0], scale); + VecMulf(pchan->pose_mat[1], scale); + VecMulf(pchan->pose_mat[2], scale); + } + if (i<ikscene->numchan) { + // big problem + ; + } +} + +//--------------------------------------------------- +// plugin interface +// +void itasc_initialize_tree(struct Scene *scene, Object *ob, float ctime) +{ + bPoseChannel *pchan; + int count = 0; + + if (ob->pose->ikdata != NULL && !(ob->pose->flag & POSE_WAS_REBUILT)) { + 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 + if (count) + create_scene(scene, ob); + 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 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; + // we need default parameters + if (!ikparam) ikparam = &DefIKParam; + + 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; + if (ob->pose->flag & POSE_GAME_ENGINE) { + timestep = ob->pose->ctime; + // limit the timestep to avoid excessive number of iteration + if (timestep > 0.2f) + timestep = 0.2f; + } + execute_scene(scene, ikscene, ikparam, ctime, timestep); + break; + } + } + } +} + +void itasc_release_tree(struct Scene *scene, struct Object *ob, float ctime) +{ + // 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; + } +} + +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); + } + } +} + +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); + } + } + } +} + +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; + } +} + |