Benoit Bolsee
0028418fe8
Disconnected bones can be translated in pose mode but this translation cannot be applied to the iTaSC representation of the armature because there is no joint associated with it. As a result, moving disconnected bones had no effect. The bug fix is in two parts: 1) manual or rna change in the armature pose will cause automatic rebuilding of the iTaSC scene 2) the iTaSC scene is now built from the current pose instead of armature rest pose
1874 lines
60 KiB
C++
1874 lines
60 KiB
C++
/*
|
|
* ***** 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) && 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 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) && 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++) ;
|
|
}
|
|
|
|
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)
|
|
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_get_constraint_target_matrix(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 {
|
|
mult_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;
|
|
mult_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_get_constraint_target_matrix(ikscene->blscene, ikscene->polarConstraint, 1, CONSTRAINT_OBTYPE_OBJECT, ikscene->blArmature, mat, 1.0);
|
|
// convert to armature space
|
|
mult_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
|
|
mult_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);
|
|
mult_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
|
|
mult_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;
|
|
}
|
|
}
|
|
|