forked from bartvdbraak/blender
Fix bug [#31588]: iTaSC does not handle armature scaling correctly. iTaSC solver operates in world reference, therefore armature scale is used to build the ik scene. But the scaling was not taken out when applying the pose at the end of the simulation.
This commit is contained in:
parent
9ec2139c81
commit
e063ea222c
@ -201,6 +201,9 @@ public:
|
|||||||
//! Adds a vector from the Vector object itself
|
//! Adds a vector from the Vector object itself
|
||||||
inline Vector& operator +=(const Vector& arg);
|
inline Vector& operator +=(const Vector& arg);
|
||||||
|
|
||||||
|
//! Multiply by a scalar
|
||||||
|
inline Vector& operator *=(double arg);
|
||||||
|
|
||||||
//! Scalar multiplication is defined
|
//! Scalar multiplication is defined
|
||||||
inline friend Vector operator*(const Vector& lhs,double rhs);
|
inline friend Vector operator*(const Vector& lhs,double rhs);
|
||||||
//! Scalar multiplication is defined
|
//! Scalar multiplication is defined
|
||||||
|
@ -143,6 +143,14 @@ Vector& Vector::operator -=(const Vector & arg)
|
|||||||
return *this;
|
return *this;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
Vector& Vector::operator *=(double arg)
|
||||||
|
{
|
||||||
|
data[0] *= arg;
|
||||||
|
data[1] *= arg;
|
||||||
|
data[2] *= arg;
|
||||||
|
return *this;
|
||||||
|
}
|
||||||
|
|
||||||
Vector Vector::Zero()
|
Vector Vector::Zero()
|
||||||
{
|
{
|
||||||
return Vector(0,0,0);
|
return Vector(0,0,0);
|
||||||
|
@ -176,6 +176,8 @@ struct IK_Scene
|
|||||||
KDL::JntArray jointArray; // buffer for storing temporary joint array
|
KDL::JntArray jointArray; // buffer for storing temporary joint array
|
||||||
iTaSC::Solver* solver;
|
iTaSC::Solver* solver;
|
||||||
Object* blArmature;
|
Object* blArmature;
|
||||||
|
float blScale; // scale of the Armature object (assume uniform scaling)
|
||||||
|
float blInvScale; // inverse of Armature object scale
|
||||||
struct bConstraint* polarConstraint;
|
struct bConstraint* polarConstraint;
|
||||||
std::vector<IK_Target*> targets;
|
std::vector<IK_Target*> targets;
|
||||||
|
|
||||||
@ -188,6 +190,7 @@ struct IK_Scene
|
|||||||
scene = NULL;
|
scene = NULL;
|
||||||
base = NULL;
|
base = NULL;
|
||||||
solver = NULL;
|
solver = NULL;
|
||||||
|
blScale = blInvScale = 1.0f;
|
||||||
blArmature = NULL;
|
blArmature = NULL;
|
||||||
numchan = 0;
|
numchan = 0;
|
||||||
numjoint = 0;
|
numjoint = 0;
|
||||||
@ -594,9 +597,10 @@ static bool base_callback(const iTaSC::Timestamp& timestamp, const iTaSC::Frame&
|
|||||||
float chanmat[4][4];
|
float chanmat[4][4];
|
||||||
copy_m4_m4(chanmat, pchan->pose_mat);
|
copy_m4_m4(chanmat, pchan->pose_mat);
|
||||||
copy_v3_v3(chanmat[3], pchan->pose_tail);
|
copy_v3_v3(chanmat[3], pchan->pose_tail);
|
||||||
// save the base as a frame too so that we can compute deformation
|
// save the base as a frame too so that we can compute deformation after simulation
|
||||||
// after simulation
|
|
||||||
ikscene->baseFrame.setValue(&chanmat[0][0]);
|
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);
|
mult_m4_m4m4(rootmat, ikscene->blArmature->obmat, chanmat);
|
||||||
}
|
}
|
||||||
else {
|
else {
|
||||||
@ -1116,14 +1120,15 @@ static IK_Scene* convert_tree(Scene *blscene, Object *ob, bPoseChannel *pchan)
|
|||||||
return NULL;
|
return NULL;
|
||||||
}
|
}
|
||||||
ikscene->blArmature = ob;
|
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 joint;
|
||||||
std::string root("root");
|
std::string root("root");
|
||||||
std::string parent;
|
std::string parent;
|
||||||
std::vector<double> weights;
|
std::vector<double> weights;
|
||||||
double weight[3];
|
double weight[3];
|
||||||
// assume uniform scaling and take Y scale as general scale for the armature
|
|
||||||
float scale = len_v3(ob->obmat[1]);
|
|
||||||
// build the array of joints corresponding to the IK chain
|
// build the array of joints corresponding to the IK chain
|
||||||
convert_channels(ikscene, tree);
|
convert_channels(ikscene, tree);
|
||||||
if (ingame) {
|
if (ingame) {
|
||||||
@ -1147,11 +1152,11 @@ static IK_Scene* convert_tree(Scene *blscene, Object *ob, bPoseChannel *pchan)
|
|||||||
fl[0][1], fl[1][1], fl[2][1],
|
fl[0][1], fl[1][1], fl[2][1],
|
||||||
fl[0][2], fl[1][2], fl[2][2]);
|
fl[0][2], fl[1][2], fl[2][2]);
|
||||||
KDL::Vector bpos(bone->head[0], bone->head[1], bone->head[2]);
|
KDL::Vector bpos(bone->head[0], bone->head[1], bone->head[2]);
|
||||||
bpos = bpos*scale;
|
bpos *= ikscene->blScale;
|
||||||
KDL::Frame head(brot, bpos);
|
KDL::Frame head(brot, bpos);
|
||||||
|
|
||||||
// rest pose length of the bone taking scaling into account
|
// rest pose length of the bone taking scaling into account
|
||||||
length= bone->length*scale;
|
length= bone->length*ikscene->blScale;
|
||||||
parent = (a > 0) ? ikscene->channels[tree->parent[a]].tail : root;
|
parent = (a > 0) ? ikscene->channels[tree->parent[a]].tail : root;
|
||||||
// first the fixed segment to the bone head
|
// first the fixed segment to the bone head
|
||||||
if (head.p.Norm() > KDL::epsilon || head.M.GetRot().Norm() > KDL::epsilon) {
|
if (head.p.Norm() > KDL::epsilon || head.M.GetRot().Norm() > KDL::epsilon) {
|
||||||
@ -1420,7 +1425,7 @@ static IK_Scene* convert_tree(Scene *blscene, Object *ob, bPoseChannel *pchan)
|
|||||||
// add the end effector
|
// add the end effector
|
||||||
// estimate the average bone length, used to clamp feedback error
|
// 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++)
|
for (bonecnt=0, bonelen=0.f, a=iktarget->channel; a>=0; a=tree->parent[a], bonecnt++)
|
||||||
bonelen += scale*tree->pchan[a]->bone->length;
|
bonelen += ikscene->blScale*tree->pchan[a]->bone->length;
|
||||||
bonelen /= bonecnt;
|
bonelen /= bonecnt;
|
||||||
|
|
||||||
// store the rest pose of the end effector to compute enforce target
|
// store the rest pose of the end effector to compute enforce target
|
||||||
@ -1527,15 +1532,23 @@ static void create_scene(Scene *scene, Object *ob)
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
static void init_scene(Object *ob)
|
/* 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) {
|
if (ob->pose->ikdata) {
|
||||||
for (IK_Scene* scene = ((IK_Data*)ob->pose->ikdata)->first;
|
for (scene = ((IK_Data*)ob->pose->ikdata)->first;
|
||||||
scene != NULL;
|
scene != NULL;
|
||||||
scene = scene->next) {
|
scene = scene->next) {
|
||||||
|
if (fabs(scene->blScale - scale) > KDL::epsilon)
|
||||||
|
return 1;
|
||||||
scene->channels[0].pchan->flag |= POSE_IKTREE;
|
scene->channels[0].pchan->flag |= POSE_IKTREE;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
static void execute_scene(Scene* blscene, IK_Scene* ikscene, bItasc* ikparam, float ctime, float frtime)
|
static void execute_scene(Scene* blscene, IK_Scene* ikscene, bItasc* ikparam, float ctime, float frtime)
|
||||||
@ -1682,6 +1695,10 @@ static void execute_scene(Scene* blscene, IK_Scene* ikscene, bItasc* ikparam, fl
|
|||||||
pchan = ikchan->pchan;
|
pchan = ikchan->pchan;
|
||||||
// tail mat
|
// tail mat
|
||||||
ikchan->frame.getValue(&pchan->pose_mat[0][0]);
|
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]);
|
copy_v3_v3(pchan->pose_tail, pchan->pose_mat[3]);
|
||||||
// shift to head
|
// shift to head
|
||||||
copy_v3_v3(yaxis, pchan->pose_mat[1]);
|
copy_v3_v3(yaxis, pchan->pose_mat[1]);
|
||||||
@ -1708,8 +1725,8 @@ void itasc_initialize_tree(struct Scene *scene, Object *ob, float ctime)
|
|||||||
int count = 0;
|
int count = 0;
|
||||||
|
|
||||||
if (ob->pose->ikdata != NULL && !(ob->pose->flag & POSE_WAS_REBUILT)) {
|
if (ob->pose->ikdata != NULL && !(ob->pose->flag & POSE_WAS_REBUILT)) {
|
||||||
init_scene(ob);
|
if (!init_scene(ob))
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
// first remove old scene
|
// first remove old scene
|
||||||
itasc_clear_data(ob->pose);
|
itasc_clear_data(ob->pose);
|
||||||
|
Loading…
Reference in New Issue
Block a user