archive/blender-archive
Archived
1
1
Fork 0
Code Pull Requests Packages Activity
This repository has been archived on 2023-10-09. You can view files and clone it, but cannot push or open issues or pull requests.
Files
3216e4b202195ed88455d1b43b348a2cf8f40d77
blender-archive/source/blender/ikplugin/intern/itasc_plugin.cpp

1876 lines
60 KiB
C++
Raw Blame History

/*
* ***** 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., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, 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 *****
*/
/** \file blender/ikplugin/intern/itasc_plugin.cpp
* \ingroup ikplugin
*/
#include <stdlib.h>
#include <string.h>
#include <vector>
// iTaSC headers
#ifdef WITH_IK_ITASC
#include "Armature.hpp"
#include "MovingFrame.hpp"
#include "CopyPose.hpp"
#include "WSDLSSolver.hpp"
#include "WDLSSolver.hpp"
#include "Scene.hpp"
#include "Cache.hpp"
#include "Distance.hpp"
#endif
#include "MEM_guardedalloc.h"
extern "C" {
#include "BIK_api.h"
#include "BLI_blenlib.h"
#include "BLI_math.h"
#include "BLI_utildefines.h"
#include "BKE_global.h"
#include "BKE_armature.h"
#include "BKE_action.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);
// one structure for each target in the scene
struct IK_Target
{
struct Scene *blscene;
iTaSC::MovingFrame* target;
iTaSC::ConstraintSet* constraint;
struct bConstraint* blenderConstraint;
struct bPoseChannel* rootChannel;
Object* owner; //for auto IK
ErrorCallback errorCallback;
std::string targetName;
std::string constraintName;
unsigned short controlType;
short channel; //index in IK channel array of channel on which this target is defined
short ee; //end effector number
bool simulation; //true when simulation mode is used (update feedback)
bool eeBlend; //end effector affected by enforce blending
float eeRest[4][4]; //end effector initial pose relative to armature
IK_Target() {
blscene = NULL;
target = NULL;
constraint = NULL;
blenderConstraint = NULL;
rootChannel = NULL;
owner = NULL;
controlType = 0;
channel = 0;
ee = 0;
eeBlend = true;
simulation = true;
targetName.reserve(32);
constraintName.reserve(32);
}
~IK_Target() {
if (constraint)
delete constraint;
if (target)
delete target;
}
};
struct IK_Channel {
bPoseChannel* pchan; // channel where we must copy matrix back
KDL::Frame frame; // frame of the bone relative to object base, not armature base
std::string tail; // segment name of the joint from which we get the bone tail
std::string head; // segment name of the joint from which we get the bone head
int parent; // index in this array of the parent channel
short jointType; // type of joint, combination of IK_SegmentFlag
char ndof; // number of joint angles for this channel
char jointValid; // set to 1 when jointValue has been computed
// for joint constraint
Object* owner; // for pose and IK param
double jointValue[4]; // computed joint value
IK_Channel() {
pchan = NULL;
parent = -1;
jointType = 0;
ndof = 0;
jointValid = 0;
owner = NULL;
jointValue[0] = 0.0;
jointValue[1] = 0.0;
jointValue[2] = 0.0;
jointValue[3] = 0.0;
}
};
struct IK_Scene {
struct Scene *blscene;
IK_Scene* next;
int numchan; // number of channel in pchan
int numjoint; // number of joint in jointArray
// array of bone information, one per channel in the tree
IK_Channel* channels;
iTaSC::Armature* armature;
iTaSC::Cache* cache;
iTaSC::Scene* scene;
iTaSC::MovingFrame* base; // armature base object
KDL::Frame baseFrame; // frame of armature base relative to blArmature
KDL::JntArray jointArray; // buffer for storing temporary joint array
iTaSC::Solver* solver;
Object* blArmature;
float blScale; // scale of the Armature object (assume uniform scaling)
float blInvScale; // inverse of Armature object scale
struct bConstraint* polarConstraint;
std::vector<IK_Target*> targets;
IK_Scene() {
blscene = NULL;
next = NULL;
channels = NULL;
armature = NULL;
cache = NULL;
scene = NULL;
base = NULL;
solver = NULL;
blScale = blInvScale = 1.0f;
blArmature = NULL;
numchan = 0;
numjoint = 0;
polarConstraint = NULL;
}
~IK_Scene() {
// delete scene first
if (scene)
delete scene;
for (std::vector<IK_Target *>::iterator it = targets.begin(); it != targets.end(); ++it)
delete (*it);
targets.clear();
if (channels)
delete[] channels;
if (solver)
delete solver;
if (armature)
delete armature;
if (base)
delete base;
// delete cache last
if (cache)
delete cache;
}
};
// type of IK joint, can be combined to list the joints corresponding to a bone
enum IK_SegmentFlag {
IK_XDOF = 1,
IK_YDOF = 2,
IK_ZDOF = 4,
IK_SWING = 8,
IK_REVOLUTE = 16,
IK_TRANSY = 32,
};
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) && BLI_listbase_is_empty(&curchan->iktree)) {
rootbone++;
continue;
}
break;
}
if (BLI_listbase_is_empty(&curchan->iktree) == false)
// Oh oh, there is already a chain starting from this channel and our chain is longer...
// Should handle this by moving the previous chain up to the beginning of our chain
// For now we just stop here
break;
}
if (!segcount) return 0;
// we reached a limit and still not the end of a previous chain, quit
if ((pchan_root->flag & POSE_CHAIN) && BLI_listbase_is_empty(&pchan_root->iktree)) return 0;
// now that we know how many segment we have, set the flag
for (rootbone = segcount, segcount = 0, curchan = pchan_tip; segcount < rootbone; segcount++, curchan = curchan->parent) {
chanlist[segcount] = curchan;
curchan->flag |= POSE_CHAIN;
}
/* setup the chain data */
/* create a target */
target = (PoseTarget *)MEM_callocN(sizeof(PoseTarget), "posetarget");
target->con = con;
// by contruction there can be only one tree per channel and each channel can be part of at most one tree.
tree = (PoseTree *)pchan_root->iktree.first;
if (tree == NULL) {
/* make new tree */
tree = (PoseTree *)MEM_callocN(sizeof(PoseTree), "posetree");
tree->iterations = data->iterations;
tree->totchannel = segcount;
tree->stretch = (data->flag & CONSTRAINT_IK_STRETCH);
tree->pchan = (bPoseChannel **)MEM_callocN(segcount * sizeof(void *), "ik tree pchan");
tree->parent = (int *)MEM_callocN(segcount * sizeof(int), "ik tree parent");
for (a = 0; a < segcount; a++) {
tree->pchan[a] = chanlist[segcount - a - 1];
tree->parent[a] = a - 1;
}
target->tip = segcount - 1;
/* AND! link the tree to the root */
BLI_addtail(&pchan_root->iktree, tree);
// new tree
treecount = 1;
}
else {
tree->iterations = MAX2(data->iterations, tree->iterations);
tree->stretch = tree->stretch && !(data->flag & CONSTRAINT_IK_STRETCH);
/* skip common pose channels and add remaining*/
size = MIN2(segcount, tree->totchannel);
a = t = 0;
while (a < size && t < tree->totchannel) {
// locate first matching channel
for (; t < tree->totchannel && tree->pchan[t] != chanlist[segcount - a - 1]; t++) ;
if (t >= tree->totchannel)
break;
for (; a < size && t < tree->totchannel && tree->pchan[t] == chanlist[segcount - a - 1]; a++, t++) ;
}
segcount = segcount - a;
target->tip = tree->totchannel + segcount - 1;
if (segcount > 0) {
for (parent = a - 1; parent < tree->totchannel; parent++)
if (tree->pchan[parent] == chanlist[segcount - 1]->parent)
break;
/* shouldn't happen, but could with dependency cycles */
if (parent == tree->totchannel)
parent = a - 1;
/* resize array */
newsize = tree->totchannel + segcount;
oldchan = tree->pchan;
oldparent = tree->parent;
tree->pchan = (bPoseChannel **)MEM_callocN(newsize * sizeof(void *), "ik tree pchan");
tree->parent = (int *)MEM_callocN(newsize * sizeof(int), "ik tree parent");
memcpy(tree->pchan, oldchan, sizeof(void *) * tree->totchannel);
memcpy(tree->parent, oldparent, sizeof(int) * tree->totchannel);
MEM_freeN(oldchan);
MEM_freeN(oldparent);
/* add new pose channels at the end, in reverse order */
for (a = 0; a < segcount; a++) {
tree->pchan[tree->totchannel + a] = chanlist[segcount - a - 1];
tree->parent[tree->totchannel + a] = tree->totchannel + a - 1;
}
tree->parent[tree->totchannel] = parent;
tree->totchannel = newsize;
}
// reusing tree
treecount = 0;
}
/* add target to the tree */
BLI_addtail(&tree->targets, target);
/* mark root channel having an IK tree */
pchan_root->flag |= POSE_IKTREE;
return treecount;
}
static bool is_cartesian_constraint(bConstraint *con)
{
//bKinematicConstraint* data=(bKinematicConstraint *)con->data;
return true;
}
static bool constraint_valid(bConstraint *con)
{
bKinematicConstraint *data = (bKinematicConstraint *)con->data;
if (data->flag & CONSTRAINT_IK_AUTO)
return true;
if (con->flag & (CONSTRAINT_DISABLE | CONSTRAINT_OFF))
return false;
if (is_cartesian_constraint(con)) {
/* cartesian space constraint */
if (data->tar == NULL)
return false;
if (data->tar->type == OB_ARMATURE && data->subtarget[0] == 0)
return false;
}
return true;
}
static int initialize_scene(Object *ob, bPoseChannel *pchan_tip)
{
bConstraint *con;
int treecount;
/* find all IK constraints and validate them */
treecount = 0;
for (con = (bConstraint *)pchan_tip->constraints.first; con; con = (bConstraint *)con->next) {
if (con->type == CONSTRAINT_TYPE_KINEMATIC) {
if (constraint_valid(con))
treecount += initialize_chain(ob, pchan_tip, con);
}
}
return treecount;
}
static IK_Data *get_ikdata(bPose *pose)
{
if (pose->ikdata)
return (IK_Data *)pose->ikdata;
pose->ikdata = MEM_callocN(sizeof(IK_Data), "iTaSC ikdata");
// here init ikdata if needed
// now that we have scene, make sure the default param are initialized
if (!DefIKParam.iksolver)
BKE_pose_itasc_init(&DefIKParam);
return (IK_Data *)pose->ikdata;
}
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;
}
#if 0
static void GetEulerXZY(const KDL::Rotation& R, double& X, double& Z, double& Y)
{
if (fabs(R(0, 1)) > 1.0 - KDL::epsilon) {
X = -KDL::sign(R(0, 1)) * KDL::atan2(R(1, 2), R(1, 0));
Z = -KDL::sign(R(0, 1)) * KDL::PI / 2;
Y = 0.0;
}
else {
X = KDL::atan2(R(2, 1), R(1, 1));
Z = KDL::atan2(-R(0, 1), KDL::sqrt(KDL::sqr(R(0, 0)) + KDL::sqr(R(0, 2))));
Y = KDL::atan2(R(0, 2), R(0, 0));
}
}
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));
}
}
#endif
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];
BKE_constraint_target_matrix_get(target->blscene, constraint, 0, CONSTRAINT_OBTYPE_OBJECT, target->owner, tarmat, 1.0);
// rootmat contains the target pose in world coordinate
// if enforce is != 1.0, blend the target position with the end effector position
// if the armature was in rest position. This information is available in eeRest
if (constraint->enforce != 1.0f && target->eeBlend) {
// eeRest is relative to the reference frame of the IK root
// get this frame in world reference
float restmat[4][4];
bPoseChannel *pchan = target->rootChannel;
if (pchan->parent) {
pchan = pchan->parent;
float chanmat[4][4];
copy_m4_m4(chanmat, pchan->pose_mat);
copy_v3_v3(chanmat[3], pchan->pose_tail);
mul_serie_m4(restmat, target->owner->obmat, chanmat, target->eeRest, NULL, NULL, NULL, NULL, NULL);
}
else {
mul_m4_m4m4(restmat, target->owner->obmat, target->eeRest);
}
// blend the target
blend_m4_m4m4(tarmat, restmat, tarmat, constraint->enforce);
}
next.setValue(&tarmat[0][0]);
return true;
}
static bool base_callback(const iTaSC::Timestamp& timestamp, const iTaSC::Frame& current, iTaSC::Frame& next, void *param)
{
IK_Scene *ikscene = (IK_Scene *)param;
// compute next armature base pose
// algorithm:
// ikscene->pchan[0] is the root channel of the tree
// if it has a parent, get the pose matrix from it and replace [3] by parent pchan->tail
// then multiply by the armature matrix to get ikscene->armature base position
bPoseChannel *pchan = ikscene->channels[0].pchan;
float rootmat[4][4];
if (pchan->parent) {
pchan = pchan->parent;
float chanmat[4][4];
copy_m4_m4(chanmat, pchan->pose_mat);
copy_v3_v3(chanmat[3], pchan->pose_tail);
// save the base as a frame too so that we can compute deformation after simulation
ikscene->baseFrame.setValue(&chanmat[0][0]);
// iTaSC armature is scaled to object scale, scale the base frame too
ikscene->baseFrame.p *= ikscene->blScale;
mul_m4_m4m4(rootmat, ikscene->blArmature->obmat, chanmat);
}
else {
copy_m4_m4(rootmat, ikscene->blArmature->obmat);
ikscene->baseFrame = iTaSC::F_identity;
}
next.setValue(&rootmat[0][0]);
// if there is a polar target (only during solving otherwise we don't have end efffector)
if (ikscene->polarConstraint && timestamp.update) {
// compute additional rotation of base frame so that armature follows the polar target
float imat[4][4]; // IK tree base inverse matrix
float polemat[4][4]; // polar target in IK tree base frame
float goalmat[4][4]; // target in IK tree base frame
float mat[4][4]; // temp matrix
bKinematicConstraint *poledata = (bKinematicConstraint *)ikscene->polarConstraint->data;
invert_m4_m4(imat, rootmat);
// polar constraint imply only one target
IK_Target *iktarget = ikscene->targets[0];
// root channel from which we take the bone initial orientation
IK_Channel &rootchan = ikscene->channels[0];
// get polar target matrix in world space
BKE_constraint_target_matrix_get(ikscene->blscene, ikscene->polarConstraint, 1, CONSTRAINT_OBTYPE_OBJECT, ikscene->blArmature, mat, 1.0);
// convert to armature space
mul_m4_m4m4(polemat, imat, mat);
// get the target in world space (was computed before as target object are defined before base object)
iktarget->target->getPose().getValue(mat[0]);
// convert to armature space
mul_m4_m4m4(goalmat, imat, mat);
// take position of target, polar target, end effector, in armature space
KDL::Vector goalpos(goalmat[3]);
KDL::Vector polepos(polemat[3]);
KDL::Vector endpos = ikscene->armature->getPose(iktarget->ee).p;
// get root bone orientation
KDL::Frame rootframe;
ikscene->armature->getRelativeFrame(rootframe, rootchan.tail);
KDL::Vector rootx = rootframe.M.UnitX();
KDL::Vector rootz = rootframe.M.UnitZ();
// and compute root bone head
double q_rest[3], q[3], length;
const KDL::Joint *joint;
const KDL::Frame *tip;
ikscene->armature->getSegment(rootchan.tail, 3, joint, q_rest[0], q[0], tip);
length = (joint->getType() == KDL::Joint::TransY) ? q[0] : tip->p(1);
KDL::Vector rootpos = rootframe.p - length *rootframe.M.UnitY();
// compute main directions
KDL::Vector dir = KDL::Normalize(endpos - rootpos);
KDL::Vector poledir = KDL::Normalize(goalpos - rootpos);
// compute up directions
KDL::Vector poleup = KDL::Normalize(polepos - rootpos);
KDL::Vector up = rootx * KDL::cos(poledata->poleangle) + rootz *KDL::sin(poledata->poleangle);
// from which we build rotation matrix
KDL::Rotation endrot, polerot;
// for the armature, using the root bone orientation
KDL::Vector x = KDL::Normalize(dir * up);
endrot.UnitX(x);
endrot.UnitY(KDL::Normalize(x * dir));
endrot.UnitZ(-dir);
// for the polar target
x = KDL::Normalize(poledir * poleup);
polerot.UnitX(x);
polerot.UnitY(KDL::Normalize(x * poledir));
polerot.UnitZ(-poledir);
// the difference between the two is the rotation we want to apply
KDL::Rotation result(polerot * endrot.Inverse());
// apply on base frame as this is an artificial additional rotation
next.M = next.M * result;
ikscene->baseFrame.M = ikscene->baseFrame.M * result;
}
return true;
}
static bool copypose_callback(const iTaSC::Timestamp& timestamp, iTaSC::ConstraintValues *const _values, unsigned int _nvalues, void *_param)
{
IK_Target *iktarget = (IK_Target *)_param;
bKinematicConstraint *condata = (bKinematicConstraint *)iktarget->blenderConstraint->data;
iTaSC::ConstraintValues *values = _values;
bItasc *ikparam = (bItasc *) iktarget->owner->pose->ikparam;
// we need default parameters
if (!ikparam)
ikparam = &DefIKParam;
if (iktarget->blenderConstraint->flag & CONSTRAINT_OFF) {
if (iktarget->controlType & iTaSC::CopyPose::CTL_POSITION) {
values->alpha = 0.0;
values->action = iTaSC::ACT_ALPHA;
values++;
}
if (iktarget->controlType & iTaSC::CopyPose::CTL_ROTATION) {
values->alpha = 0.0;
values->action = iTaSC::ACT_ALPHA;
values++;
}
}
else {
if (iktarget->controlType & iTaSC::CopyPose::CTL_POSITION) {
// update error
values->alpha = condata->weight;
values->action = iTaSC::ACT_ALPHA | iTaSC::ACT_FEEDBACK;
values->feedback = (iktarget->simulation) ? ikparam->feedback : ANIM_FEEDBACK;
values++;
}
if (iktarget->controlType & iTaSC::CopyPose::CTL_ROTATION) {
// update error
values->alpha = condata->orientweight;
values->action = iTaSC::ACT_ALPHA | iTaSC::ACT_FEEDBACK;
values->feedback = (iktarget->simulation) ? ikparam->feedback : ANIM_FEEDBACK;
values++;
}
}
return true;
}
static void copypose_error(const iTaSC::ConstraintValues *values, unsigned int nvalues, IK_Target *iktarget)
{
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) */
eulO_to_mat3(rmat, chan->eul, chan->rotmode);
}
else if (chan->rotmode == ROT_MODE_AXISANGLE) {
/* axis-angle - stored in quaternion data, but not really that great for 3D-changing orientations */
axis_angle_to_mat3(rmat, &chan->quat[1], chan->quat[0]);
}
else {
/* quats are normalized before use to eliminate scaling issues */
normalize_qt(chan->quat);
quat_to_mat3(rmat, chan->quat);
}
KDL::Rotation jointRot(
rmat[0][0], rmat[1][0], rmat[2][0],
rmat[0][1], rmat[1][1], rmat[2][1],
rmat[0][2], rmat[1][2], rmat[2][2]);
GetJointRotation(jointRot, ikchan->jointType, ikchan->jointValue);
ikchan->jointValid = 1;
}
// determine which part of jointValue is used for this joint
// closely related to the way the joints are defined
switch (ikchan->jointType & ~IK_TRANSY) {
case IK_XDOF:
case IK_YDOF:
case IK_ZDOF:
dof = 0;
break;
case IK_XDOF | IK_YDOF:
// X + Y
dof = (_values[0].id == iTaSC::Armature::ID_JOINT_RX) ? 0 : 1;
break;
case IK_SWING:
// XZ
dof = 0;
break;
case IK_YDOF | IK_ZDOF:
// Z + Y
dof = (_values[0].id == iTaSC::Armature::ID_JOINT_RZ) ? 0 : 1;
break;
case IK_SWING | IK_YDOF:
// XZ + Y
dof = (_values[0].id == iTaSC::Armature::ID_JOINT_RY) ? 2 : 0;
break;
case IK_REVOLUTE:
dof = 0;
break;
default:
dof = -1;
break;
}
if (dof >= 0) {
for (unsigned int i = 0; i < _nvalues; i++, dof++) {
_values[i].values[0].yd = ikchan->jointValue[dof];
_values[i].alpha = chan->ikrotweight;
_values[i].feedback = ikparam->feedback;
}
}
return true;
}
// build array of joint corresponding to IK chain
static int convert_channels(IK_Scene *ikscene, PoseTree *tree, float ctime)
{
IK_Channel *ikchan;
bPoseChannel *pchan;
int a, flag, njoint;
njoint = 0;
for (a = 0, ikchan = ikscene->channels; a < ikscene->numchan; ++a, ++ikchan) {
pchan = tree->pchan[a];
ikchan->pchan = pchan;
ikchan->parent = (a > 0) ? tree->parent[a] : -1;
ikchan->owner = ikscene->blArmature;
// the constraint and channels must be applied before we build the iTaSC scene,
// this is because some of the pose data (e.g. pose head) don't have corresponding
// joint angles and can't be applied to the iTaSC armature dynamically
if (!(pchan->flag & POSE_DONE))
BKE_pose_where_is_bone(ikscene->blscene, ikscene->blArmature, pchan, ctime, 1);
// tell blender that this channel was controlled by IK, it's cleared on each BKE_pose_where_is()
pchan->flag |= (POSE_DONE | POSE_CHAIN);
/* set DoF flag */
flag = 0;
if (!(pchan->ikflag & BONE_IK_NO_XDOF) && !(pchan->ikflag & BONE_IK_NO_XDOF_TEMP) &&
(!(pchan->ikflag & BONE_IK_XLIMIT) || pchan->limitmin[0] < 0.f || pchan->limitmax[0] > 0.f))
{
flag |= IK_XDOF;
}
if (!(pchan->ikflag & BONE_IK_NO_YDOF) && !(pchan->ikflag & BONE_IK_NO_YDOF_TEMP) &&
(!(pchan->ikflag & BONE_IK_YLIMIT) || pchan->limitmin[1] < 0.f || pchan->limitmax[1] > 0.f))
{
flag |= IK_YDOF;
}
if (!(pchan->ikflag & BONE_IK_NO_ZDOF) && !(pchan->ikflag & BONE_IK_NO_ZDOF_TEMP) &&
(!(pchan->ikflag & BONE_IK_ZLIMIT) || pchan->limitmin[2] < 0.f || pchan->limitmax[2] > 0.f))
{
flag |= IK_ZDOF;
}
if (tree->stretch && (pchan->ikstretch > 0.0)) {
flag |= IK_TRANSY;
}
/*
* Logic to create the segments:
* RX,RY,RZ = rotational joints with no length
* RY(tip) = rotational joints with a fixed length arm = (0,length,0)
* TY = translational joint on Y axis
* F(pos) = fixed joint with an arm at position pos
* Conversion rule of the above flags:
* - ==> F(tip)
* X ==> RX(tip)
* Y ==> RY(tip)
* Z ==> RZ(tip)
* XY ==> RX+RY(tip)
* XZ ==> RX+RZ(tip)
* YZ ==> RZ+RY(tip)
* XYZ ==> full spherical unless there are limits, in which case RX+RZ+RY(tip)
* In case of stretch, tip=(0,0,0) and there is an additional TY joint
* The frame at last of these joints represents the tail of the bone.
* The head is computed by a reverse translation on Y axis of the bone length
* or in case of TY joint, by the frame at previous joint.
* In case of separation of bones, there is an additional F(head) joint
*
* Computing rest pose and length is complicated: the solver works in world space
* Here is the logic:
* rest position is computed only from bone->bone_mat.
* bone length is computed from bone->length multiplied by the scaling factor of
* the armature. Non-uniform scaling will give bad result!
*/
switch (flag & (IK_XDOF | IK_YDOF | IK_ZDOF)) {
default:
ikchan->jointType = 0;
ikchan->ndof = 0;
break;
case IK_XDOF:
// RX only, get the X rotation
ikchan->jointType = IK_XDOF;
ikchan->ndof = 1;
break;
case IK_YDOF:
// RY only, get the Y rotation
ikchan->jointType = IK_YDOF;
ikchan->ndof = 1;
break;
case IK_ZDOF:
// RZ only, get the Zz rotation
ikchan->jointType = IK_ZDOF;
ikchan->ndof = 1;
break;
case IK_XDOF | IK_YDOF:
ikchan->jointType = IK_XDOF | IK_YDOF;
ikchan->ndof = 2;
break;
case IK_XDOF | IK_ZDOF:
// RX+RZ
ikchan->jointType = IK_SWING;
ikchan->ndof = 2;
break;
case IK_YDOF | IK_ZDOF:
// RZ+RY
ikchan->jointType = IK_ZDOF | IK_YDOF;
ikchan->ndof = 2;
break;
case IK_XDOF | IK_YDOF | IK_ZDOF:
// spherical joint
if (pchan->ikflag & (BONE_IK_XLIMIT | BONE_IK_YLIMIT | BONE_IK_ZLIMIT))
// decompose in a Swing+RotY joint
ikchan->jointType = IK_SWING | IK_YDOF;
else
ikchan->jointType = IK_REVOLUTE;
ikchan->ndof = 3;
break;
}
if (flag & IK_TRANSY) {
ikchan->jointType |= IK_TRANSY;
ikchan->ndof++;
}
njoint += ikchan->ndof;
}
// njoint is the joint coordinate, create the Joint Array
ikscene->jointArray.resize(njoint);
ikscene->numjoint = njoint;
return njoint;
}
// compute array of joint value corresponding to current pose
static void convert_pose(IK_Scene *ikscene)
{
KDL::Rotation boneRot;
bPoseChannel *pchan;
IK_Channel *ikchan;
Bone *bone;
float rmat[4][4]; // rest pose of bone with parent taken into account
float bmat[4][4]; // difference
float scale;
double *rot;
int a, joint;
// assume uniform scaling and take Y scale as general scale for the armature
scale = len_v3(ikscene->blArmature->obmat[1]);
rot = ikscene->jointArray(0);
for (joint = a = 0, ikchan = ikscene->channels; a < ikscene->numchan && joint < ikscene->numjoint; ++a, ++ikchan) {
pchan = ikchan->pchan;
bone = pchan->bone;
if (pchan->parent) {
unit_m4(bmat);
mul_m4_m4m3(bmat, pchan->parent->pose_mat, bone->bone_mat);
}
else {
copy_m4_m4(bmat, bone->arm_mat);
}
invert_m4_m4(rmat, bmat);
mul_m4_m4m4(bmat, rmat, pchan->pose_mat);
normalize_m4(bmat);
boneRot.setValue(bmat[0]);
GetJointRotation(boneRot, ikchan->jointType, rot);
if (ikchan->jointType & IK_TRANSY) {
// compute actual length
rot[ikchan->ndof - 1] = len_v3v3(pchan->pose_tail, pchan->pose_head) * scale;
}
rot += ikchan->ndof;
joint += ikchan->ndof;
}
}
// compute array of joint value corresponding to current pose
static void BKE_pose_rest(IK_Scene *ikscene)
{
bPoseChannel *pchan;
IK_Channel *ikchan;
Bone *bone;
float scale;
double *rot;
int a, joint;
// assume uniform scaling and take Y scale as general scale for the armature
scale = len_v3(ikscene->blArmature->obmat[1]);
// rest pose is 0
SetToZero(ikscene->jointArray);
// except for transY joints
rot = ikscene->jointArray(0);
for (joint = a = 0, ikchan = ikscene->channels; a < ikscene->numchan && joint < ikscene->numjoint; ++a, ++ikchan) {
pchan = ikchan->pchan;
bone = pchan->bone;
if (ikchan->jointType & IK_TRANSY)
rot[ikchan->ndof - 1] = bone->length * scale;
rot += ikchan->ndof;
joint += ikchan->ndof;
}
}
static IK_Scene *convert_tree(Scene *blscene, Object *ob, bPoseChannel *pchan, float ctime)
{
PoseTree *tree = (PoseTree *)pchan->iktree.first;
PoseTarget *target;
bKinematicConstraint *condata;
bConstraint *polarcon;
bItasc *ikparam;
iTaSC::Armature *arm;
iTaSC::Scene *scene;
IK_Scene *ikscene;
IK_Channel *ikchan;
KDL::Frame initPose;
KDL::Rotation boneRot;
Bone *bone;
int a, numtarget;
unsigned int t;
float length;
bool ret = true, ingame;
double *rot;
float start[3];
if (tree->totchannel == 0)
return NULL;
ikscene = new IK_Scene;
ikscene->blscene = blscene;
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;
// assume uniform scaling and take Y scale as general scale for the armature
ikscene->blScale = len_v3(ob->obmat[1]);
ikscene->blInvScale = (ikscene->blScale < KDL::epsilon) ? 0.0f : 1.0f / ikscene->blScale;
std::string joint;
std::string root("root");
std::string parent;
std::vector<double> weights;
double weight[3];
// build the array of joints corresponding to the IK chain
convert_channels(ikscene, tree, ctime);
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
BKE_pose_rest(ikscene);
}
rot = ikscene->jointArray(0);
for (a = 0, ikchan = ikscene->channels; a < tree->totchannel; ++a, ++ikchan) {
pchan = ikchan->pchan;
bone = pchan->bone;
KDL::Frame tip(iTaSC::F_identity);
// compute the position and rotation of the head from previous segment
Vector3 *fl = bone->bone_mat;
KDL::Rotation brot(
fl[0][0], fl[1][0], fl[2][0],
fl[0][1], fl[1][1], fl[2][1],
fl[0][2], fl[1][2], fl[2][2]);
// if the bone is disconnected, the head is movable in pose mode
// take that into account by using pose matrix instead of bone
// Note that pose is expressed in armature space, convert to previous bone space
{
float R_parmat[3][3];
float iR_parmat[3][3];
if (pchan->parent)
copy_m3_m4(R_parmat, pchan->parent->pose_mat);
else
unit_m3(R_parmat);
if (pchan->parent)
sub_v3_v3v3(start, pchan->pose_head, pchan->parent->pose_tail);
else
start[0] = start[1] = start[2] = 0.0f;
invert_m3_m3(iR_parmat, R_parmat);
normalize_m3(iR_parmat);
mul_m3_v3(iR_parmat, start);
}
KDL::Vector bpos(start[0], start[1], start[2]);
bpos *= ikscene->blScale;
KDL::Frame head(brot, bpos);
// rest pose length of the bone taking scaling into account
length = bone->length * ikscene->blScale;
parent = (a > 0) ? ikscene->channels[tree->parent[a]].tail : root;
// first the fixed segment to the bone head
if (!(ikchan->pchan->bone->flag & BONE_CONNECTED) || head.M.GetRot().Norm() > KDL::epsilon) {
joint = bone->name;
joint += ":H";
ret = arm->addSegment(joint, parent, KDL::Joint::None, 0.0, head);
parent = joint;
}
if (!(ikchan->jointType & IK_TRANSY)) {
// fixed length, put it in tip
tip.p[1] = length;
}
joint = bone->name;
weight[0] = (1.0 - pchan->stiffness[0]);
weight[1] = (1.0 - pchan->stiffness[1]);
weight[2] = (1.0 - pchan->stiffness[2]);
switch (ikchan->jointType & ~IK_TRANSY) {
case 0:
// fixed bone
joint += ":F";
ret = arm->addSegment(joint, parent, KDL::Joint::None, 0.0, tip);
break;
case IK_XDOF:
// RX only, get the X rotation
joint += ":RX";
ret = arm->addSegment(joint, parent, KDL::Joint::RotX, rot[0], tip);
weights.push_back(weight[0]);
break;
case IK_YDOF:
// RY only, get the Y rotation
joint += ":RY";
ret = arm->addSegment(joint, parent, KDL::Joint::RotY, rot[0], tip);
weights.push_back(weight[1]);
break;
case IK_ZDOF:
// RZ only, get the Zz rotation
joint += ":RZ";
ret = arm->addSegment(joint, parent, KDL::Joint::RotZ, rot[0], tip);
weights.push_back(weight[2]);
break;
case IK_XDOF | IK_YDOF:
joint += ":RX";
ret = arm->addSegment(joint, parent, KDL::Joint::RotX, rot[0]);
weights.push_back(weight[0]);
if (ret) {
parent = joint;
joint = bone->name;
joint += ":RY";
ret = arm->addSegment(joint, parent, KDL::Joint::RotY, rot[1], tip);
weights.push_back(weight[1]);
}
break;
case IK_SWING:
joint += ":SW";
ret = arm->addSegment(joint, parent, KDL::Joint::Swing, rot[0], tip);
weights.push_back(weight[0]);
weights.push_back(weight[2]);
break;
case IK_YDOF | IK_ZDOF:
// RZ+RY
joint += ":RZ";
ret = arm->addSegment(joint, parent, KDL::Joint::RotZ, rot[0]);
weights.push_back(weight[2]);
if (ret) {
parent = joint;
joint = bone->name;
joint += ":RY";
ret = arm->addSegment(joint, parent, KDL::Joint::RotY, rot[1], tip);
weights.push_back(weight[1]);
}
break;
case IK_SWING | IK_YDOF:
// decompose in a Swing+RotY joint
joint += ":SW";
ret = arm->addSegment(joint, parent, KDL::Joint::Swing, rot[0]);
weights.push_back(weight[0]);
weights.push_back(weight[2]);
if (ret) {
parent = joint;
joint = bone->name;
joint += ":RY";
ret = arm->addSegment(joint, parent, KDL::Joint::RotY, rot[2], tip);
weights.push_back(weight[1]);
}
break;
case IK_REVOLUTE:
joint += ":SJ";
ret = arm->addSegment(joint, parent, KDL::Joint::Sphere, rot[0], tip);
weights.push_back(weight[0]);
weights.push_back(weight[1]);
weights.push_back(weight[2]);
break;
}
if (ret && (ikchan->jointType & IK_TRANSY)) {
parent = joint;
joint = bone->name;
joint += ":TY";
ret = arm->addSegment(joint, parent, KDL::Joint::TransY, rot[ikchan->ndof - 1]);
const float ikstretch = pchan->ikstretch * pchan->ikstretch;
/* why invert twice here? */
weight[1] = (1.0 - min_ff(1.0 - ikstretch, 1.0f - 0.001f));
weights.push_back(weight[1]);
}
if (!ret)
// error making the armature??
break;
// joint points to the segment that correspond to the bone per say
ikchan->tail = joint;
ikchan->head = parent;
// in case of error
ret = false;
if ((ikchan->jointType & IK_XDOF) && (pchan->ikflag & (BONE_IK_XLIMIT | BONE_IK_ROTCTL))) {
joint = bone->name;
joint += ":RX";
if (pchan->ikflag & BONE_IK_XLIMIT) {
if (arm->addLimitConstraint(joint, 0, pchan->limitmin[0], pchan->limitmax[0]) < 0)
break;
}
if (pchan->ikflag & BONE_IK_ROTCTL) {
if (arm->addConstraint(joint, joint_callback, ikchan, false, false) < 0)
break;
}
}
if ((ikchan->jointType & IK_YDOF) && (pchan->ikflag & (BONE_IK_YLIMIT | BONE_IK_ROTCTL))) {
joint = bone->name;
joint += ":RY";
if (pchan->ikflag & BONE_IK_YLIMIT) {
if (arm->addLimitConstraint(joint, 0, pchan->limitmin[1], pchan->limitmax[1]) < 0)
break;
}
if (pchan->ikflag & BONE_IK_ROTCTL) {
if (arm->addConstraint(joint, joint_callback, ikchan, false, false) < 0)
break;
}
}
if ((ikchan->jointType & IK_ZDOF) && (pchan->ikflag & (BONE_IK_ZLIMIT | BONE_IK_ROTCTL))) {
joint = bone->name;
joint += ":RZ";
if (pchan->ikflag & BONE_IK_ZLIMIT) {
if (arm->addLimitConstraint(joint, 0, pchan->limitmin[2], pchan->limitmax[2]) < 0)
break;
}
if (pchan->ikflag & BONE_IK_ROTCTL) {
if (arm->addConstraint(joint, joint_callback, ikchan, false, false) < 0)
break;
}
}
if ((ikchan->jointType & IK_SWING) && (pchan->ikflag & (BONE_IK_XLIMIT | BONE_IK_ZLIMIT | BONE_IK_ROTCTL))) {
joint = bone->name;
joint += ":SW";
if (pchan->ikflag & BONE_IK_XLIMIT) {
if (arm->addLimitConstraint(joint, 0, pchan->limitmin[0], pchan->limitmax[0]) < 0)
break;
}
if (pchan->ikflag & BONE_IK_ZLIMIT) {
if (arm->addLimitConstraint(joint, 1, pchan->limitmin[2], pchan->limitmax[2]) < 0)
break;
}
if (pchan->ikflag & BONE_IK_ROTCTL) {
if (arm->addConstraint(joint, joint_callback, ikchan, false, false) < 0)
break;
}
}
if ((ikchan->jointType & IK_REVOLUTE) && (pchan->ikflag & BONE_IK_ROTCTL)) {
joint = bone->name;
joint += ":SJ";
if (arm->addConstraint(joint, joint_callback, ikchan, false, false) < 0)
break;
}
// no error, so restore
ret = true;
rot += ikchan->ndof;
}
if (!ret) {
delete ikscene;
return NULL;
}
// for each target, we need to add an end effector in the armature
for (numtarget = 0, polarcon = NULL, ret = true, target = (PoseTarget *)tree->targets.first; target; target = (PoseTarget *)target->next) {
condata = (bKinematicConstraint *)target->con->data;
pchan = tree->pchan[target->tip];
if (is_cartesian_constraint(target->con)) {
// add the end effector
IK_Target *iktarget = new IK_Target();
ikscene->targets.push_back(iktarget);
iktarget->ee = arm->addEndEffector(ikscene->channels[target->tip].tail);
if (iktarget->ee == -1) {
ret = false;
break;
}
// initialize all the fields that we can set at this time
iktarget->blenderConstraint = target->con;
iktarget->channel = target->tip;
iktarget->simulation = (ikparam->flag & ITASC_SIMULATION);
iktarget->rootChannel = ikscene->channels[0].pchan;
iktarget->owner = ob;
iktarget->targetName = pchan->bone->name;
iktarget->targetName += ":T:";
iktarget->targetName += target->con->name;
iktarget->constraintName = pchan->bone->name;
iktarget->constraintName += ":C:";
iktarget->constraintName += target->con->name;
numtarget++;
if (condata->poletar)
// this constraint has a polar target
polarcon = target->con;
}
}
// deal with polar target if any
if (numtarget == 1 && polarcon) {
ikscene->polarConstraint = polarcon;
}
// we can now add the armature
// the armature is based on a moving frame.
// initialize with the correct position in case there is no cache
base_callback(iTaSC::Timestamp(), iTaSC::F_identity, initPose, ikscene);
ikscene->base = new iTaSC::MovingFrame(initPose);
ikscene->base->setCallback(base_callback, ikscene);
std::string armname;
armname = ob->id.name;
armname += ":B";
ret = scene->addObject(armname, ikscene->base);
armname = ob->id.name;
armname += ":AR";
if (ret)
ret = scene->addObject(armname, ikscene->armature, ikscene->base);
if (!ret) {
delete ikscene;
return NULL;
}
// set the weight
e_matrix& Wq = arm->getWq();
assert(Wq.cols() == (int)weights.size());
for (int q = 0; q < Wq.cols(); q++)
Wq(q, q) = weights[q];
// get the inverse rest pose frame of the base to compute relative rest pose of end effectors
// this is needed to handle the enforce parameter
// ikscene->pchan[0] is the root channel of the tree
// if it has no parent, then it's just the identify Frame
float invBaseFrame[4][4];
pchan = ikscene->channels[0].pchan;
if (pchan->parent) {
// it has a parent, get the pose matrix from it
float baseFrame[4][4];
pchan = pchan->parent;
copy_m4_m4(baseFrame, pchan->bone->arm_mat);
// move to the tail and scale to get rest pose of armature base
copy_v3_v3(baseFrame[3], pchan->bone->arm_tail);
invert_m4_m4(invBaseFrame, baseFrame);
}
else {
unit_m4(invBaseFrame);
}
// finally add the constraint
for (t = 0; t < ikscene->targets.size(); t++) {
IK_Target *iktarget = ikscene->targets[t];
iktarget->blscene = blscene;
condata = (bKinematicConstraint *)iktarget->blenderConstraint->data;
pchan = tree->pchan[iktarget->channel];
unsigned int controltype, bonecnt;
double bonelen;
float mat[4][4];
// add the end effector
// estimate the average bone length, used to clamp feedback error
for (bonecnt = 0, bonelen = 0.f, a = iktarget->channel; a >= 0; a = tree->parent[a], bonecnt++)
bonelen += ikscene->blScale * tree->pchan[a]->bone->length;
bonelen /= bonecnt;
// store the rest pose of the end effector to compute enforce target
copy_m4_m4(mat, pchan->bone->arm_mat);
copy_v3_v3(mat[3], pchan->bone->arm_tail);
// get the rest pose relative to the armature base
mul_m4_m4m4(iktarget->eeRest, invBaseFrame, mat);
iktarget->eeBlend = (!ikscene->polarConstraint && condata->type == CONSTRAINT_IK_COPYPOSE) ? true : false;
// use target_callback to make sure the initPose includes enforce coefficient
target_callback(iTaSC::Timestamp(), iTaSC::F_identity, initPose, iktarget);
iktarget->target = new iTaSC::MovingFrame(initPose);
iktarget->target->setCallback(target_callback, iktarget);
ret = scene->addObject(iktarget->targetName, iktarget->target);
if (!ret)
break;
switch (condata->type) {
case CONSTRAINT_IK_COPYPOSE:
controltype = 0;
if (condata->flag & CONSTRAINT_IK_ROT) {
if (!(condata->flag & CONSTRAINT_IK_NO_ROT_X))
controltype |= iTaSC::CopyPose::CTL_ROTATIONX;
if (!(condata->flag & CONSTRAINT_IK_NO_ROT_Y))
controltype |= iTaSC::CopyPose::CTL_ROTATIONY;
if (!(condata->flag & CONSTRAINT_IK_NO_ROT_Z))
controltype |= iTaSC::CopyPose::CTL_ROTATIONZ;
}
if (condata->flag & CONSTRAINT_IK_POS) {
if (!(condata->flag & CONSTRAINT_IK_NO_POS_X))
controltype |= iTaSC::CopyPose::CTL_POSITIONX;
if (!(condata->flag & CONSTRAINT_IK_NO_POS_Y))
controltype |= iTaSC::CopyPose::CTL_POSITIONY;
if (!(condata->flag & CONSTRAINT_IK_NO_POS_Z))
controltype |= iTaSC::CopyPose::CTL_POSITIONZ;
}
if (controltype) {
iktarget->constraint = new iTaSC::CopyPose(controltype, controltype, bonelen);
// set the gain
if (controltype & iTaSC::CopyPose::CTL_POSITION)
iktarget->constraint->setControlParameter(iTaSC::CopyPose::ID_POSITION, iTaSC::ACT_ALPHA, condata->weight);
if (controltype & iTaSC::CopyPose::CTL_ROTATION)
iktarget->constraint->setControlParameter(iTaSC::CopyPose::ID_ROTATION, iTaSC::ACT_ALPHA, condata->orientweight);
iktarget->constraint->registerCallback(copypose_callback, iktarget);
iktarget->errorCallback = copypose_error;
iktarget->controlType = controltype;
// add the constraint
if (condata->flag & CONSTRAINT_IK_TARGETAXIS)
ret = scene->addConstraintSet(iktarget->constraintName, iktarget->constraint, iktarget->targetName, armname, "", ikscene->channels[iktarget->channel].tail);
else
ret = scene->addConstraintSet(iktarget->constraintName, iktarget->constraint, armname, iktarget->targetName, ikscene->channels[iktarget->channel].tail);
}
break;
case CONSTRAINT_IK_DISTANCE:
iktarget->constraint = new iTaSC::Distance(bonelen);
iktarget->constraint->setControlParameter(iTaSC::Distance::ID_DISTANCE, iTaSC::ACT_VALUE, condata->dist);
iktarget->constraint->registerCallback(distance_callback, iktarget);
iktarget->errorCallback = distance_error;
// we can update the weight on each substep
iktarget->constraint->substep(true);
// add the constraint
ret = scene->addConstraintSet(iktarget->constraintName, iktarget->constraint, armname, iktarget->targetName, ikscene->channels[iktarget->channel].tail);
break;
}
if (!ret)
break;
}
if (!ret ||
!scene->addCache(ikscene->cache) ||
!scene->addSolver(ikscene->solver) ||
!scene->initialize())
{
delete ikscene;
ikscene = NULL;
}
return ikscene;
}
static void create_scene(Scene *scene, Object *ob, float ctime)
{
bPoseChannel *pchan;
// create the IK scene
for (pchan = (bPoseChannel *)ob->pose->chanbase.first; pchan; pchan = (bPoseChannel *)pchan->next) {
// by construction there is only one tree
PoseTree *tree = (PoseTree *)pchan->iktree.first;
if (tree) {
IK_Data *ikdata = get_ikdata(ob->pose);
// convert tree in iTaSC::Scene
IK_Scene *ikscene = convert_tree(scene, ob, pchan, ctime);
if (ikscene) {
ikscene->next = ikdata->first;
ikdata->first = ikscene;
}
// delete the trees once we are done
while (tree) {
BLI_remlink(&pchan->iktree, tree);
BLI_freelistN(&tree->targets);
if (tree->pchan) MEM_freeN(tree->pchan);
if (tree->parent) MEM_freeN(tree->parent);
if (tree->basis_change) MEM_freeN(tree->basis_change);
MEM_freeN(tree);
tree = (PoseTree *)pchan->iktree.first;
}
}
}
}
/* returns 1 if scaling has changed and tree must be reinitialized */
static int init_scene(Object *ob)
{
// check also if scaling has changed
float scale = len_v3(ob->obmat[1]);
IK_Scene *scene;
if (ob->pose->ikdata) {
for (scene = ((IK_Data *)ob->pose->ikdata)->first;
scene != NULL;
scene = scene->next)
{
if (fabs(scene->blScale - scale) > KDL::epsilon)
return 1;
scene->channels[0].pchan->flag |= POSE_IKTREE;
}
}
return 0;
}
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 BKE_pose_where_is()
ikchan->pchan->flag |= (POSE_DONE | POSE_CHAIN);
ikchan->jointValid = 0;
}
}
else {
// in animation mode, we must get the bone position from action and constraints
for (i = 0, ikchan = ikscene->channels; i < ikscene->numchan; i++, ++ikchan) {
if (!(ikchan->pchan->flag & POSE_DONE))
BKE_pose_where_is_bone(blscene, ikscene->blArmature, ikchan->pchan, ctime, 1);
// tell blender that this channel was controlled by IK, it's cleared on each BKE_pose_where_is()
ikchan->pchan->flag |= (POSE_DONE | POSE_CHAIN);
ikchan->jointValid = 0;
}
}
// only run execute the scene if at least one of our target is enabled
for (i = ikscene->targets.size(); i > 0; --i) {
IK_Target *iktarget = ikscene->targets[i - 1];
if (!(iktarget->blenderConstraint->flag & CONSTRAINT_OFF))
break;
}
if (i == 0 && ikscene->armature->getNrOfConstraints() == 0)
// all constraint disabled
return;
// compute timestep
double timestamp = ctime * frtime + 2147483.648;
double timestep = frtime;
bool reiterate = (ikparam->flag & ITASC_REITERATION) ? true : false;
int numstep = (ikparam->flag & ITASC_AUTO_STEP) ? 0 : ikparam->numstep;
bool simulation = true;
if (ikparam->flag & ITASC_SIMULATION) {
ikscene->solver->setParam(iTaSC::Solver::DLS_QMAX, ikparam->maxvel);
}
else {
// in animation mode we start from the pose after action and constraint
convert_pose(ikscene);
ikscene->armature->setJointArray(ikscene->jointArray);
// and we don't handle velocity
reiterate = true;
simulation = false;
// time is virtual, so take fixed value for velocity parameters (see itasc_update_param)
// and choose 1s timestep to allow having velocity parameters in radiant
timestep = 1.0;
// use auto setup to let the solver test the variation of the joints
numstep = 0;
}
if (ikscene->cache && !reiterate && simulation) {
iTaSC::CacheTS sts, cts;
sts = cts = (iTaSC::CacheTS)(timestamp * 1000.0 + 0.5);
if (ikscene->cache->getPreviousCacheItem(ikscene->armature, 0, &cts) == NULL || cts == 0) {
// the cache is empty before this time, reiterate
if (ikparam->flag & ITASC_INITIAL_REITERATION)
reiterate = true;
}
else {
// can take the cache as a start point.
sts -= cts;
timestep = sts / 1000.0;
}
}
// don't cache if we are reiterating because we don't want to destroy the cache unnecessarily
ikscene->scene->update(timestamp, timestep, numstep, false, !reiterate, simulation);
if (reiterate) {
// how many times do we reiterate?
for (i = 0; i < ikparam->numiter; i++) {
if (ikscene->armature->getMaxJointChange() < ikparam->precision ||
ikscene->armature->getMaxEndEffectorChange() < ikparam->precision)
{
break;
}
ikscene->scene->update(timestamp, timestep, numstep, true, false, simulation);
}
if (simulation) {
// one more fake iteration to cache
ikscene->scene->update(timestamp, 0.0, 1, true, true, true);
}
}
// compute constraint error
for (i = ikscene->targets.size(); i > 0; --i) {
IK_Target *iktarget = ikscene->targets[i - 1];
if (!(iktarget->blenderConstraint->flag & CONSTRAINT_OFF) && iktarget->constraint) {
unsigned int nvalues;
const iTaSC::ConstraintValues *values;
values = iktarget->constraint->getControlParameters(&nvalues);
iktarget->errorCallback(values, nvalues, iktarget);
}
}
// Apply result to bone:
// walk the ikscene->channels
// for each, get the Frame of the joint corresponding to the bone relative to its parent
// combine the parent and the joint frame to get the frame relative to armature
// a backward translation of the bone length gives the head
// if TY, compute the scale as the ratio of the joint length with rest pose length
iTaSC::Armature *arm = ikscene->armature;
KDL::Frame frame;
double q_rest[3], q[3];
const KDL::Joint *joint;
const KDL::Frame *tip;
bPoseChannel *pchan;
float scale;
float length;
float yaxis[3];
for (i = 0, ikchan = ikscene->channels; i < ikscene->numchan; ++i, ++ikchan) {
if (i == 0) {
if (!arm->getRelativeFrame(frame, ikchan->tail))
break;
// this frame is relative to base, make it relative to object
ikchan->frame = ikscene->baseFrame * frame;
}
else {
if (!arm->getRelativeFrame(frame, ikchan->tail, ikscene->channels[ikchan->parent].tail))
break;
// combine with parent frame to get frame relative to object
ikchan->frame = ikscene->channels[ikchan->parent].frame * frame;
}
// ikchan->frame is the tail frame relative to object
// get bone length
if (!arm->getSegment(ikchan->tail, 3, joint, q_rest[0], q[0], tip))
break;
if (joint->getType() == KDL::Joint::TransY) {
// stretch bones have a TY joint, compute the scale
scale = (float)(q[0] / q_rest[0]);
// the length is the joint itself
length = (float)q[0];
}
else {
scale = 1.0f;
// for fixed bone, the length is in the tip (always along Y axis)
length = tip->p(1);
}
// ready to compute the pose mat
pchan = ikchan->pchan;
// tail mat
ikchan->frame.getValue(&pchan->pose_mat[0][0]);
// the scale of the object was included in the ik scene, take it out now
// because the pose channels are relative to the object
mul_v3_fl(pchan->pose_mat[3], ikscene->blInvScale);
length *= ikscene->blInvScale;
copy_v3_v3(pchan->pose_tail, pchan->pose_mat[3]);
// shift to head
copy_v3_v3(yaxis, pchan->pose_mat[1]);
mul_v3_fl(yaxis, length);
sub_v3_v3v3(pchan->pose_mat[3], pchan->pose_mat[3], yaxis);
copy_v3_v3(pchan->pose_head, pchan->pose_mat[3]);
// add scale
mul_v3_fl(pchan->pose_mat[0], scale);
mul_v3_fl(pchan->pose_mat[1], scale);
mul_v3_fl(pchan->pose_mat[2], scale);
}
if (i < ikscene->numchan) {
// big problem
;
}
}
//---------------------------------------------------
// plugin interface
//
void itasc_initialize_tree(struct Scene *scene, Object *ob, float ctime)
{
bPoseChannel *pchan;
int count = 0;
if (ob->pose->ikdata != NULL && !(ob->pose->flag & POSE_WAS_REBUILT)) {
if (!init_scene(ob))
return;
}
// first remove old scene
itasc_clear_data(ob->pose);
// we should handle all the constraint and mark them all disabled
// for blender but we'll start with the IK constraint alone
for (pchan = (bPoseChannel *)ob->pose->chanbase.first; pchan; pchan = (bPoseChannel *)pchan->next) {
if (pchan->constflag & PCHAN_HAS_IK)
count += initialize_scene(ob, pchan);
}
// if at least one tree, create the scenes from the PoseTree stored in the channels
// postpone until execute_tree: this way the pose constraint are included
//if (count)
// create_scene(scene, ob, ctime);
//itasc_update_param(ob->pose);
// make sure we don't rebuilt until the user changes something important
ob->pose->flag &= ~POSE_WAS_REBUILT;
}
void itasc_execute_tree(struct Scene *scene, struct Object *ob, struct bPoseChannel *pchan, float ctime)
{
if (!ob->pose->ikdata) {
// IK tree not yet created, no it now
create_scene(scene, ob, ctime);
itasc_update_param(ob->pose);
}
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;
}
}
View Git Blame Copy Permalink