2015 lines
66 KiB
C++
2015 lines
66 KiB
C++
/*
|
|
* 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.
|
|
* Original author: Benoit Bolsee
|
|
*/
|
|
|
|
/** \file
|
|
* \ingroup ikplugin
|
|
*/
|
|
|
|
#include <cmath>
|
|
#include <cstdlib>
|
|
#include <cstring>
|
|
#include <vector>
|
|
|
|
/* iTaSC headers */
|
|
#ifdef WITH_IK_ITASC
|
|
# include "Armature.hpp"
|
|
# include "Cache.hpp"
|
|
# include "CopyPose.hpp"
|
|
# include "Distance.hpp"
|
|
# include "MovingFrame.hpp"
|
|
# include "Scene.hpp"
|
|
# include "WDLSSolver.hpp"
|
|
# include "WSDLSSolver.hpp"
|
|
#endif
|
|
|
|
#include "MEM_guardedalloc.h"
|
|
|
|
#include "BIK_api.h"
|
|
#include "BLI_blenlib.h"
|
|
#include "BLI_math.h"
|
|
#include "BLI_utildefines.h"
|
|
|
|
#include "BKE_action.h"
|
|
#include "BKE_armature.h"
|
|
#include "BKE_constraint.h"
|
|
#include "BKE_global.h"
|
|
#include "DNA_action_types.h"
|
|
#include "DNA_armature_types.h"
|
|
#include "DNA_constraint_types.h"
|
|
#include "DNA_object_types.h"
|
|
#include "DNA_scene_types.h"
|
|
|
|
#include "itasc_plugin.h"
|
|
|
|
/* default parameters */
|
|
static 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;
|
|
};
|
|
|
|
using Vector3 = float[3];
|
|
using Vector4 = float[4];
|
|
struct IK_Target;
|
|
using ErrorCallback = void (*)(const iTaSC::ConstraintValues *values,
|
|
unsigned int nvalues,
|
|
IK_Target *iktarget);
|
|
|
|
/* one structure for each target in the scene */
|
|
struct IK_Target {
|
|
struct Depsgraph *bldepsgraph;
|
|
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()
|
|
{
|
|
bldepsgraph = nullptr;
|
|
blscene = nullptr;
|
|
target = nullptr;
|
|
constraint = nullptr;
|
|
blenderConstraint = nullptr;
|
|
rootChannel = nullptr;
|
|
owner = nullptr;
|
|
controlType = 0;
|
|
channel = 0;
|
|
ee = 0;
|
|
eeBlend = true;
|
|
simulation = true;
|
|
targetName.reserve(32);
|
|
constraintName.reserve(32);
|
|
}
|
|
~IK_Target()
|
|
{
|
|
delete constraint;
|
|
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 = nullptr;
|
|
parent = -1;
|
|
jointType = 0;
|
|
ndof = 0;
|
|
jointValid = 0;
|
|
owner = nullptr;
|
|
jointValue[0] = 0.0;
|
|
jointValue[1] = 0.0;
|
|
jointValue[2] = 0.0;
|
|
jointValue[3] = 0.0;
|
|
}
|
|
};
|
|
|
|
struct IK_Scene {
|
|
struct Depsgraph *bldepsgraph;
|
|
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()
|
|
{
|
|
bldepsgraph = nullptr;
|
|
blscene = nullptr;
|
|
next = nullptr;
|
|
channels = nullptr;
|
|
armature = nullptr;
|
|
cache = nullptr;
|
|
scene = nullptr;
|
|
base = nullptr;
|
|
solver = nullptr;
|
|
blScale = blInvScale = 1.0f;
|
|
blArmature = nullptr;
|
|
numchan = 0;
|
|
numjoint = 0;
|
|
polarConstraint = nullptr;
|
|
}
|
|
|
|
~IK_Scene()
|
|
{
|
|
/* delete scene first */
|
|
delete scene;
|
|
for (IK_Target *target : targets) {
|
|
delete target;
|
|
}
|
|
targets.clear();
|
|
delete[] channels;
|
|
delete solver;
|
|
delete armature;
|
|
delete base;
|
|
/* delete cache last */
|
|
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 = nullptr, *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 construction 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 == nullptr) {
|
|
/* 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++) {
|
|
/* pass */
|
|
}
|
|
if (t >= tree->totchannel) {
|
|
break;
|
|
}
|
|
for (; a < size && t < tree->totchannel && tree->pchan[t] == chanlist[segcount - a - 1];
|
|
a++, t++) {
|
|
/* pass */
|
|
}
|
|
}
|
|
|
|
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 == nullptr) {
|
|
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));
|
|
}
|
|
if (axis == 1) {
|
|
return KDL::atan2(-R(0, 2), t);
|
|
}
|
|
|
|
return -KDL::atan2(R(0, 1), R(0, 0));
|
|
}
|
|
|
|
if (axis == 0) {
|
|
return -KDL::atan2(-R(2, 1), R(1, 1));
|
|
}
|
|
if (axis == 1) {
|
|
return KDL::atan2(-R(0, 2), t);
|
|
}
|
|
|
|
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 ×tamp,
|
|
const iTaSC::Frame ¤t,
|
|
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->bldepsgraph,
|
|
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_m4_series(restmat, target->owner->obmat, chanmat, target->eeRest);
|
|
}
|
|
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 ×tamp,
|
|
const iTaSC::Frame ¤t,
|
|
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->bldepsgraph,
|
|
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 ×tamp,
|
|
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 ×tamp,
|
|
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 ×tamp,
|
|
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 split 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 gimbal 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(struct Depsgraph *depsgraph,
|
|
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(depsgraph, ikscene->blscene, ikscene->blArmature, pchan, ctime, true);
|
|
}
|
|
/* 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.0f ||
|
|
pchan->limitmax[0] > 0.0f)) {
|
|
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.0f ||
|
|
pchan->limitmax[1] > 0.0f)) {
|
|
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.0f ||
|
|
pchan->limitmax[2] > 0.0f)) {
|
|
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(
|
|
struct Depsgraph *depsgraph, 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;
|
|
Bone *bone;
|
|
int a, numtarget;
|
|
unsigned int t;
|
|
float length;
|
|
bool ret = true;
|
|
double *rot;
|
|
float start[3];
|
|
|
|
if (tree->totchannel == 0) {
|
|
return nullptr;
|
|
}
|
|
|
|
ikscene = new IK_Scene;
|
|
ikscene->blscene = blscene;
|
|
ikscene->bldepsgraph = depsgraph;
|
|
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;
|
|
|
|
if (!ikparam) {
|
|
/* you must have our own copy */
|
|
ikparam = &DefIKParam;
|
|
}
|
|
|
|
if (ikparam->flag & ITASC_SIMULATION) {
|
|
/* 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 nullptr;
|
|
}
|
|
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(depsgraph, ikscene, tree, ctime);
|
|
/* 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 nullptr;
|
|
}
|
|
/* for each target, we need to add an end effector in the armature */
|
|
for (numtarget = 0, polarcon = nullptr, 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 nullptr;
|
|
}
|
|
/* 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;
|
|
iktarget->bldepsgraph = depsgraph;
|
|
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.0f, 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 = nullptr;
|
|
}
|
|
return ikscene;
|
|
}
|
|
|
|
static void create_scene(struct Depsgraph *depsgraph, 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(depsgraph, 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 != nullptr; 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(struct Depsgraph *depsgraph,
|
|
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 constraint 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(
|
|
depsgraph, blscene, ikscene->blArmature, ikchan->pchan, ctime, true);
|
|
}
|
|
/* 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)std::round(timestamp * 1000.0);
|
|
if (ikscene->cache->getPreviousCacheItem(ikscene->armature, 0, &cts) == nullptr || 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 Depsgraph *depsgraph,
|
|
struct Scene *scene,
|
|
Object *ob,
|
|
float ctime)
|
|
{
|
|
bPoseChannel *pchan;
|
|
int count = 0;
|
|
|
|
if (ob->pose->ikdata != nullptr && !(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(depsgraph, 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 Depsgraph *depsgraph,
|
|
struct Scene *scene,
|
|
Object *ob,
|
|
bPoseChannel *pchan_root,
|
|
float ctime)
|
|
{
|
|
if (ob->pose->ikdata) {
|
|
IK_Data *ikdata = (IK_Data *)ob->pose->ikdata;
|
|
bItasc *ikparam = (bItasc *)ob->pose->ikparam;
|
|
/* we need default parameters */
|
|
if (!ikparam) {
|
|
ikparam = &DefIKParam;
|
|
}
|
|
|
|
for (IK_Scene *ikscene = ikdata->first; ikscene; ikscene = ikscene->next) {
|
|
if (ikscene->channels[0].pchan == pchan_root) {
|
|
float timestep = scene->r.frs_sec_base / scene->r.frs_sec;
|
|
execute_scene(depsgraph, 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 = nullptr;
|
|
}
|
|
}
|
|
|
|
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(nullptr, 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 == nullptr) {
|
|
return;
|
|
}
|
|
|
|
switch (data->type) {
|
|
case CONSTRAINT_IK_COPYPOSE:
|
|
case CONSTRAINT_IK_DISTANCE:
|
|
/* cartesian space constraint */
|
|
break;
|
|
}
|
|
}
|