237
236
static int initialize_chain(Object *ob, bPoseChannel *pchan_tip, bConstraint *con)
239
bPoseChannel *curchan, *pchan_root=NULL, *chanlist[256], **oldchan;
238
bPoseChannel *curchan, *pchan_root = NULL, *chanlist[256], **oldchan;
241
240
PoseTarget *target;
242
241
bKinematicConstraint *data;
243
int a, t, segcount= 0, size, newsize, *oldparent, parent, rootbone, treecount;
245
data=(bKinematicConstraint*)con->data;
242
int a, t, segcount = 0, size, newsize, *oldparent, parent, rootbone, treecount;
244
data = (bKinematicConstraint *)con->data;
247
246
/* exclude tip from chain? */
248
247
if (!(data->flag & CONSTRAINT_IK_TIP))
249
pchan_tip= pchan_tip->parent;
248
pchan_tip = pchan_tip->parent;
251
250
rootbone = data->rootbone;
252
251
/* Find the chain's root & count the segments needed */
253
for (curchan = pchan_tip; curchan; curchan=curchan->parent) {
252
for (curchan = pchan_tip; curchan; curchan = curchan->parent) {
254
253
pchan_root = curchan;
256
if (++segcount > 255) // 255 is weak
255
if (++segcount > 255) // 255 is weak
259
if (segcount==rootbone) {
260
// reached this end of the chain but if the chain is overlapping with a
258
if (segcount == rootbone) {
259
// reached this end of the chain but if the chain is overlapping with a
261
260
// previous one, we must go back up to the root of the other chain
262
261
if ((curchan->flag & POSE_CHAIN) && curchan->iktree.first == NULL) {
269
268
if (curchan->iktree.first != NULL)
270
// Oh oh, there is already a chain starting from this channel and our chain is longer...
269
// Oh oh, there is already a chain starting from this channel and our chain is longer...
271
270
// Should handle this by moving the previous chain up to the beginning of our chain
272
271
// For now we just stop here
277
276
if ((pchan_root->flag & POSE_CHAIN) && pchan_root->iktree.first == NULL) return 0;
279
278
// now that we know how many segment we have, set the flag
280
for (rootbone = segcount, segcount = 0, curchan = pchan_tip; segcount < rootbone; segcount++, curchan=curchan->parent) {
281
chanlist[segcount]=curchan;
279
for (rootbone = segcount, segcount = 0, curchan = pchan_tip; segcount < rootbone; segcount++, curchan = curchan->parent) {
280
chanlist[segcount] = curchan;
282
281
curchan->flag |= POSE_CHAIN;
285
284
/* setup the chain data */
286
285
/* create a target */
287
target= (PoseTarget*)MEM_callocN(sizeof(PoseTarget), "posetarget");
286
target = (PoseTarget *)MEM_callocN(sizeof(PoseTarget), "posetarget");
289
288
// by contruction there can be only one tree per channel and each channel can be part of at most one tree.
290
tree = (PoseTree*)pchan_root->iktree.first;
289
tree = (PoseTree *)pchan_root->iktree.first;
293
292
/* make new tree */
294
tree= (PoseTree*)MEM_callocN(sizeof(PoseTree), "posetree");
293
tree = (PoseTree *)MEM_callocN(sizeof(PoseTree), "posetree");
296
tree->iterations= data->iterations;
297
tree->totchannel= segcount;
295
tree->iterations = data->iterations;
296
tree->totchannel = segcount;
298
297
tree->stretch = (data->flag & CONSTRAINT_IK_STRETCH);
300
tree->pchan= (bPoseChannel**)MEM_callocN(segcount*sizeof(void*), "ik tree pchan");
301
tree->parent= (int*)MEM_callocN(segcount*sizeof(int), "ik tree parent");
302
for (a=0; a<segcount; a++) {
303
tree->pchan[a]= chanlist[segcount-a-1];
304
tree->parent[a]= a-1;
299
tree->pchan = (bPoseChannel **)MEM_callocN(segcount * sizeof(void *), "ik tree pchan");
300
tree->parent = (int *)MEM_callocN(segcount * sizeof(int), "ik tree parent");
301
for (a = 0; a < segcount; a++) {
302
tree->pchan[a] = chanlist[segcount - a - 1];
303
tree->parent[a] = a - 1;
306
target->tip= segcount-1;
305
target->tip = segcount - 1;
308
307
/* AND! link the tree to the root */
309
308
BLI_addtail(&pchan_root->iktree, tree);
314
tree->iterations= MAX2(data->iterations, tree->iterations);
315
tree->stretch= tree->stretch && !(data->flag & CONSTRAINT_IK_STRETCH);
313
tree->iterations = MAX2(data->iterations, tree->iterations);
314
tree->stretch = tree->stretch && !(data->flag & CONSTRAINT_IK_STRETCH);
317
316
/* skip common pose channels and add remaining*/
318
size= MIN2(segcount, tree->totchannel);
317
size = MIN2(segcount, tree->totchannel);
320
while (a<size && t<tree->totchannel) {
319
while (a < size && t < tree->totchannel) {
321
320
// locate first matching channel
322
for (;t<tree->totchannel && tree->pchan[t]!=chanlist[segcount-a-1];t++);
323
if (t>=tree->totchannel)
321
for (; t < tree->totchannel && tree->pchan[t] != chanlist[segcount - a - 1]; t++) ;
322
if (t >= tree->totchannel)
325
for (; a<size && t<tree->totchannel && tree->pchan[t]==chanlist[segcount-a-1]; a++, t++);
324
for (; a < size && t < tree->totchannel && tree->pchan[t] == chanlist[segcount - a - 1]; a++, t++) ;
328
segcount= segcount-a;
329
target->tip= tree->totchannel + segcount - 1;
327
segcount = segcount - a;
328
target->tip = tree->totchannel + segcount - 1;
331
330
if (segcount > 0) {
332
331
for (parent = a - 1; parent < tree->totchannel; parent++)
333
if (tree->pchan[parent] == chanlist[segcount-1]->parent)
332
if (tree->pchan[parent] == chanlist[segcount - 1]->parent)
336
335
/* shouldn't happen, but could with dependency cycles */
337
336
if (parent == tree->totchannel)
340
339
/* resize array */
341
newsize= tree->totchannel + segcount;
342
oldchan= tree->pchan;
343
oldparent= tree->parent;
340
newsize = tree->totchannel + segcount;
341
oldchan = tree->pchan;
342
oldparent = tree->parent;
345
tree->pchan= (bPoseChannel**)MEM_callocN(newsize*sizeof(void*), "ik tree pchan");
346
tree->parent= (int*)MEM_callocN(newsize*sizeof(int), "ik tree parent");
347
memcpy(tree->pchan, oldchan, sizeof(void*)*tree->totchannel);
348
memcpy(tree->parent, oldparent, sizeof(int)*tree->totchannel);
344
tree->pchan = (bPoseChannel **)MEM_callocN(newsize * sizeof(void *), "ik tree pchan");
345
tree->parent = (int *)MEM_callocN(newsize * sizeof(int), "ik tree parent");
346
memcpy(tree->pchan, oldchan, sizeof(void *) * tree->totchannel);
347
memcpy(tree->parent, oldparent, sizeof(int) * tree->totchannel);
349
348
MEM_freeN(oldchan);
350
349
MEM_freeN(oldparent);
352
351
/* add new pose channels at the end, in reverse order */
353
for (a=0; a<segcount; a++) {
354
tree->pchan[tree->totchannel+a]= chanlist[segcount-a-1];
355
tree->parent[tree->totchannel+a]= tree->totchannel+a-1;
352
for (a = 0; a < segcount; a++) {
353
tree->pchan[tree->totchannel + a] = chanlist[segcount - a - 1];
354
tree->parent[tree->totchannel + a] = tree->totchannel + a - 1;
357
tree->parent[tree->totchannel]= parent;
359
tree->totchannel= newsize;
356
tree->parent[tree->totchannel] = parent;
358
tree->totchannel = newsize;
410
409
return treecount;
413
static IK_Data* get_ikdata(bPose *pose)
412
static IK_Data *get_ikdata(bPose *pose)
415
414
if (pose->ikdata)
416
return (IK_Data*)pose->ikdata;
415
return (IK_Data *)pose->ikdata;
417
416
pose->ikdata = MEM_callocN(sizeof(IK_Data), "iTaSC ikdata");
418
417
// here init ikdata if needed
419
418
// now that we have scene, make sure the default param are initialized
420
419
if (!DefIKParam.iksolver)
421
init_pose_itasc(&DefIKParam);
420
BKE_pose_itasc_init(&DefIKParam);
423
return (IK_Data*)pose->ikdata;
422
return (IK_Data *)pose->ikdata;
425
424
static double EulerAngleFromMatrix(const KDL::Rotation& R, int axis)
427
double t = KDL::sqrt(R(0,0)*R(0,0) + R(0,1)*R(0,1));
426
double t = KDL::sqrt(R(0, 0) * R(0, 0) + R(0, 1) * R(0, 1));
429
if (t > 16.0*KDL::epsilon) {
430
if (axis == 0) return -KDL::atan2(R(1,2), R(2,2));
431
else if (axis == 1) return KDL::atan2(-R(0,2), t);
432
else return -KDL::atan2(R(0,1), R(0,0));
428
if (t > 16.0 * KDL::epsilon) {
429
if (axis == 0) return -KDL::atan2(R(1, 2), R(2, 2));
430
else if (axis == 1) return KDL::atan2(-R(0, 2), t);
431
else return -KDL::atan2(R(0, 1), R(0, 0));
435
if (axis == 0) return -KDL::atan2(-R(2,1), R(1,1));
436
else if (axis == 1) return KDL::atan2(-R(0,2), t);
434
if (axis == 0) return -KDL::atan2(-R(2, 1), R(1, 1));
435
else if (axis == 1) return KDL::atan2(-R(0, 2), t);
437
436
else return 0.0f;
454
453
// compute twist parameter
458
T = KDL::Rotation::RotX(-angle);
461
T = KDL::Rotation::RotY(-angle);
464
T = KDL::Rotation::RotZ(-angle);
457
T = KDL::Rotation::RotX(-angle);
460
T = KDL::Rotation::RotY(-angle);
463
T = KDL::Rotation::RotZ(-angle);
474
static void GetEulerXZY(const KDL::Rotation& R, double& X,double& Z,double& Y)
473
static void GetEulerXZY(const KDL::Rotation& R, double& X, double& Z, double& Y)
476
if (fabs(R(0,1)) > 1.0 - KDL::epsilon ) {
477
X = -KDL::sign(R(0,1)) * KDL::atan2(R(1,2), R(1,0));
478
Z = -KDL::sign(R(0,1)) * KDL::PI / 2;
475
if (fabs(R(0, 1)) > 1.0 - KDL::epsilon) {
476
X = -KDL::sign(R(0, 1)) * KDL::atan2(R(1, 2), R(1, 0));
477
Z = -KDL::sign(R(0, 1)) * KDL::PI / 2;
482
X = KDL::atan2(R(2,1), R(1,1));
483
Z = KDL::atan2(-R(0,1), KDL::sqrt( KDL::sqr(R(0,0)) + KDL::sqr(R(0,2))));
484
Y = KDL::atan2(R(0,2), R(0,0));
481
X = KDL::atan2(R(2, 1), R(1, 1));
482
Z = KDL::atan2(-R(0, 1), KDL::sqrt(KDL::sqr(R(0, 0)) + KDL::sqr(R(0, 2))));
483
Y = KDL::atan2(R(0, 2), R(0, 0));
488
static void GetEulerXYZ(const KDL::Rotation& R, double& X,double& Y,double& Z)
487
static void GetEulerXYZ(const KDL::Rotation& R, double& X, double& Y, double& Z)
490
if (fabs(R(0,2)) > 1.0 - KDL::epsilon ) {
491
X = KDL::sign(R(0,2)) * KDL::atan2(-R(1,0), R(1,1));
492
Y = KDL::sign(R(0,2)) * KDL::PI / 2;
489
if (fabs(R(0, 2)) > 1.0 - KDL::epsilon) {
490
X = KDL::sign(R(0, 2)) * KDL::atan2(-R(1, 0), R(1, 1));
491
Y = KDL::sign(R(0, 2)) * KDL::PI / 2;
496
X = KDL::atan2(-R(1,2), R(2,2));
497
Y = KDL::atan2(R(0,2), KDL::sqrt( KDL::sqr(R(0,0)) + KDL::sqr(R(0,1))));
498
Z = KDL::atan2(-R(0,1), R(0,0));
495
X = KDL::atan2(-R(1, 2), R(2, 2));
496
Y = KDL::atan2(R(0, 2), KDL::sqrt(KDL::sqr(R(0, 0)) + KDL::sqr(R(0, 1))));
497
Z = KDL::atan2(-R(0, 1), R(0, 0));
503
static void GetJointRotation(KDL::Rotation& boneRot, int type, double* rot)
502
static void GetJointRotation(KDL::Rotation& boneRot, int type, double *rot)
505
switch (type & ~IK_TRANSY)
508
// fixed bone, no joint
511
// RX only, get the X rotation
512
rot[0] = EulerAngleFromMatrix(boneRot, 0);
515
// RY only, get the Y rotation
516
rot[0] = ComputeTwist(boneRot);
519
// RZ only, get the Z rotation
520
rot[0] = EulerAngleFromMatrix(boneRot, 2);
522
case IK_XDOF|IK_YDOF:
523
rot[1] = ComputeTwist(boneRot);
524
RemoveEulerAngleFromMatrix(boneRot, rot[1], 1);
525
rot[0] = EulerAngleFromMatrix(boneRot, 0);
529
boneRot.GetXZRot().GetValue(rot);
531
case IK_YDOF|IK_ZDOF:
533
rot[1] = ComputeTwist(boneRot);
534
RemoveEulerAngleFromMatrix(boneRot, rot[1], 1);
535
rot[0] = EulerAngleFromMatrix(boneRot, 2);
537
case IK_SWING|IK_YDOF:
538
rot[2] = ComputeTwist(boneRot);
539
RemoveEulerAngleFromMatrix(boneRot, rot[2], 1);
540
boneRot.GetXZRot().GetValue(rot);
543
boneRot.GetRot().GetValue(rot);
504
switch (type & ~IK_TRANSY) {
506
// fixed bone, no joint
509
// RX only, get the X rotation
510
rot[0] = EulerAngleFromMatrix(boneRot, 0);
513
// RY only, get the Y rotation
514
rot[0] = ComputeTwist(boneRot);
517
// RZ only, get the Z rotation
518
rot[0] = EulerAngleFromMatrix(boneRot, 2);
520
case IK_XDOF | IK_YDOF:
521
rot[1] = ComputeTwist(boneRot);
522
RemoveEulerAngleFromMatrix(boneRot, rot[1], 1);
523
rot[0] = EulerAngleFromMatrix(boneRot, 0);
527
boneRot.GetXZRot().GetValue(rot);
529
case IK_YDOF | IK_ZDOF:
531
rot[1] = ComputeTwist(boneRot);
532
RemoveEulerAngleFromMatrix(boneRot, rot[1], 1);
533
rot[0] = EulerAngleFromMatrix(boneRot, 2);
535
case IK_SWING | IK_YDOF:
536
rot[2] = ComputeTwist(boneRot);
537
RemoveEulerAngleFromMatrix(boneRot, rot[2], 1);
538
boneRot.GetXZRot().GetValue(rot);
541
boneRot.GetRot().GetValue(rot);
548
546
static bool target_callback(const iTaSC::Timestamp& timestamp, const iTaSC::Frame& current, iTaSC::Frame& next, void *param)
550
IK_Target* target = (IK_Target*)param;
548
IK_Target *target = (IK_Target *)param;
551
549
// compute next target position
552
550
// get target matrix from constraint.
553
bConstraint* constraint = (bConstraint*)target->blenderConstraint;
551
bConstraint *constraint = (bConstraint *)target->blenderConstraint;
554
552
float tarmat[4][4];
556
get_constraint_target_matrix(target->blscene, constraint, 0, CONSTRAINT_OBTYPE_OBJECT, target->owner, tarmat, 1.0);
554
BKE_get_constraint_target_matrix(target->blscene, constraint, 0, CONSTRAINT_OBTYPE_OBJECT, target->owner, tarmat, 1.0);
558
556
// rootmat contains the target pose in world coordinate
559
557
// if enforce is != 1.0, blend the target position with the end effector position
639
638
KDL::Vector rootz = rootframe.M.UnitZ();
640
639
// and compute root bone head
641
640
double q_rest[3], q[3], length;
642
const KDL::Joint* joint;
643
const KDL::Frame* tip;
641
const KDL::Joint *joint;
642
const KDL::Frame *tip;
644
643
ikscene->armature->getSegment(rootchan.tail, 3, joint, q_rest[0], q[0], tip);
645
644
length = (joint->getType() == KDL::Joint::TransY) ? q[0] : tip->p(1);
646
KDL::Vector rootpos = rootframe.p - length*rootframe.M.UnitY();
645
KDL::Vector rootpos = rootframe.p - length *rootframe.M.UnitY();
648
// compute main directions
647
// compute main directions
649
648
KDL::Vector dir = KDL::Normalize(endpos - rootpos);
650
KDL::Vector poledir = KDL::Normalize(goalpos-rootpos);
649
KDL::Vector poledir = KDL::Normalize(goalpos - rootpos);
651
650
// compute up directions
652
KDL::Vector poleup = KDL::Normalize(polepos-rootpos);
653
KDL::Vector up = rootx*KDL::cos(poledata->poleangle) + rootz*KDL::sin(poledata->poleangle);
651
KDL::Vector poleup = KDL::Normalize(polepos - rootpos);
652
KDL::Vector up = rootx * KDL::cos(poledata->poleangle) + rootz *KDL::sin(poledata->poleangle);
654
653
// from which we build rotation matrix
655
654
KDL::Rotation endrot, polerot;
656
655
// for the armature, using the root bone orientation
657
KDL::Vector x = KDL::Normalize(dir*up);
656
KDL::Vector x = KDL::Normalize(dir * up);
659
endrot.UnitY(KDL::Normalize(x*dir));
658
endrot.UnitY(KDL::Normalize(x * dir));
660
659
endrot.UnitZ(-dir);
661
// for the polar target
662
x = KDL::Normalize(poledir*poleup);
660
// for the polar target
661
x = KDL::Normalize(poledir * poleup);
663
662
polerot.UnitX(x);
664
polerot.UnitY(KDL::Normalize(x*poledir));
663
polerot.UnitY(KDL::Normalize(x * poledir));
665
664
polerot.UnitZ(-poledir);
666
665
// the difference between the two is the rotation we want to apply
667
KDL::Rotation result(polerot*endrot.Inverse());
666
KDL::Rotation result(polerot * endrot.Inverse());
668
667
// apply on base frame as this is an artificial additional rotation
669
next.M = next.M*result;
670
ikscene->baseFrame.M = ikscene->baseFrame.M*result;
668
next.M = next.M * result;
669
ikscene->baseFrame.M = ikscene->baseFrame.M * result;
675
static bool copypose_callback(const iTaSC::Timestamp& timestamp, iTaSC::ConstraintValues* const _values, unsigned int _nvalues, void* _param)
674
static bool copypose_callback(const iTaSC::Timestamp& timestamp, iTaSC::ConstraintValues *const _values, unsigned int _nvalues, void *_param)
677
IK_Target* iktarget =(IK_Target*)_param;
676
IK_Target *iktarget = (IK_Target *)_param;
678
677
bKinematicConstraint *condata = (bKinematicConstraint *)iktarget->blenderConstraint->data;
679
iTaSC::ConstraintValues* values = _values;
680
bItasc* ikparam = (bItasc*) iktarget->owner->pose->ikparam;
678
iTaSC::ConstraintValues *values = _values;
679
bItasc *ikparam = (bItasc *) iktarget->owner->pose->ikparam;
682
681
// we need default parameters
684
683
ikparam = &DefIKParam;
686
685
if (iktarget->blenderConstraint->flag & CONSTRAINT_OFF) {
717
static void copypose_error(const iTaSC::ConstraintValues* values, unsigned int nvalues, IK_Target* iktarget)
716
static void copypose_error(const iTaSC::ConstraintValues *values, unsigned int nvalues, IK_Target *iktarget)
719
iTaSC::ConstraintSingleValue* value;
718
iTaSC::ConstraintSingleValue *value;
723
722
if (iktarget->controlType & iTaSC::CopyPose::CTL_POSITION) {
725
for (i=0, error=0.0, value=values->values; i<values->number; ++i, ++value)
724
for (i = 0, error = 0.0, value = values->values; i < values->number; ++i, ++value)
726
725
error += KDL::sqr(value->y - value->yd);
727
726
iktarget->blenderConstraint->lin_error = (float)KDL::sqrt(error);
730
729
if (iktarget->controlType & iTaSC::CopyPose::CTL_ROTATION) {
732
for (i=0, error=0.0, value=values->values; i<values->number; ++i, ++value)
731
for (i = 0, error = 0.0, value = values->values; i < values->number; ++i, ++value)
733
732
error += KDL::sqr(value->y - value->yd);
734
733
iktarget->blenderConstraint->rot_error = (float)KDL::sqrt(error);
739
static bool distance_callback(const iTaSC::Timestamp& timestamp, iTaSC::ConstraintValues* const _values, unsigned int _nvalues, void* _param)
738
static bool distance_callback(const iTaSC::Timestamp& timestamp, iTaSC::ConstraintValues *const _values, unsigned int _nvalues, void *_param)
741
IK_Target* iktarget =(IK_Target*)_param;
740
IK_Target *iktarget = (IK_Target *)_param;
742
741
bKinematicConstraint *condata = (bKinematicConstraint *)iktarget->blenderConstraint->data;
743
iTaSC::ConstraintValues* values = _values;
744
bItasc* ikparam = (bItasc*) iktarget->owner->pose->ikparam;
742
iTaSC::ConstraintValues *values = _values;
743
bItasc *ikparam = (bItasc *) iktarget->owner->pose->ikparam;
745
744
// we need default parameters
747
746
ikparam = &DefIKParam;
749
748
// update weight according to mode
806
805
if (chan->rotmode > 0) {
807
806
/* euler rotations (will cause gimble lock, but this can be alleviated a bit with rotation orders) */
808
eulO_to_mat3( rmat,chan->eul, chan->rotmode);
807
eulO_to_mat3(rmat, chan->eul, chan->rotmode);
810
809
else if (chan->rotmode == ROT_MODE_AXISANGLE) {
811
810
/* axis-angle - stored in quaternion data, but not really that great for 3D-changing orientations */
812
axis_angle_to_mat3( rmat,&chan->quat[1], chan->quat[0]);
811
axis_angle_to_mat3(rmat, &chan->quat[1], chan->quat[0]);
815
/* quats are normalised before use to eliminate scaling issues */
814
/* quats are normalized before use to eliminate scaling issues */
816
815
normalize_qt(chan->quat);
817
quat_to_mat3( rmat,chan->quat);
816
quat_to_mat3(rmat, chan->quat);
819
818
KDL::Rotation jointRot(
820
rmat[0][0], rmat[1][0], rmat[2][0],
821
rmat[0][1], rmat[1][1], rmat[2][1],
822
rmat[0][2], rmat[1][2], rmat[2][2]);
819
rmat[0][0], rmat[1][0], rmat[2][0],
820
rmat[0][1], rmat[1][1], rmat[2][1],
821
rmat[0][2], rmat[1][2], rmat[2][2]);
823
822
GetJointRotation(jointRot, ikchan->jointType, ikchan->jointValue);
824
823
ikchan->jointValid = 1;
826
825
// determine which part of jointValue is used for this joint
827
826
// closely related to the way the joints are defined
828
switch (ikchan->jointType & ~IK_TRANSY)
835
case IK_XDOF|IK_YDOF:
837
dof = (_values[0].id == iTaSC::Armature::ID_JOINT_RX) ? 0 : 1;
843
case IK_YDOF|IK_ZDOF:
845
dof = (_values[0].id == iTaSC::Armature::ID_JOINT_RZ) ? 0 : 1;
847
case IK_SWING|IK_YDOF:
849
dof = (_values[0].id == iTaSC::Armature::ID_JOINT_RY) ? 2 : 0;
827
switch (ikchan->jointType & ~IK_TRANSY) {
833
case IK_XDOF | IK_YDOF:
835
dof = (_values[0].id == iTaSC::Armature::ID_JOINT_RX) ? 0 : 1;
841
case IK_YDOF | IK_ZDOF:
843
dof = (_values[0].id == iTaSC::Armature::ID_JOINT_RZ) ? 0 : 1;
845
case IK_SWING | IK_YDOF:
847
dof = (_values[0].id == iTaSC::Armature::ID_JOINT_RY) ? 2 : 0;
859
for (unsigned int i=0; i<_nvalues; i++, dof++) {
857
for (unsigned int i = 0; i < _nvalues; i++, dof++) {
860
858
_values[i].values[0].yd = ikchan->jointValue[dof];
861
859
_values[i].alpha = chan->ikrotweight;
862
860
_values[i].feedback = ikparam->feedback;
868
866
// build array of joint corresponding to IK chain
869
static int convert_channels(IK_Scene *ikscene, PoseTree *tree)
867
static int convert_channels(IK_Scene *ikscene, PoseTree *tree, float ctime)
871
869
IK_Channel *ikchan;
872
870
bPoseChannel *pchan;
873
871
int a, flag, njoint;
876
for (a=0, ikchan = ikscene->channels; a<ikscene->numchan; ++a, ++ikchan) {
877
pchan= tree->pchan[a];
874
for (a = 0, ikchan = ikscene->channels; a < ikscene->numchan; ++a, ++ikchan) {
875
pchan = tree->pchan[a];
878
876
ikchan->pchan = pchan;
879
ikchan->parent = (a>0) ? tree->parent[a] : -1;
877
ikchan->parent = (a > 0) ? tree->parent[a] : -1;
880
878
ikchan->owner = ikscene->blArmature;
880
// the constraint and channels must be applied before we build the iTaSC scene,
881
// this is because some of the pose data (e.g. pose head) don't have corresponding
882
// joint angles and can't be applied to the iTaSC armature dynamically
883
if (!(pchan->flag & POSE_DONE))
884
BKE_pose_where_is_bone(ikscene->blscene, ikscene->blArmature, pchan, ctime, 1);
885
// tell blender that this channel was controlled by IK, it's cleared on each BKE_pose_where_is()
886
pchan->flag |= (POSE_DONE | POSE_CHAIN);
882
888
/* set DoF flag */
884
if (!(pchan->ikflag & BONE_IK_NO_XDOF) && !(pchan->ikflag & BONE_IK_NO_XDOF_TEMP) &&
885
(!(pchan->ikflag & BONE_IK_XLIMIT) || pchan->limitmin[0]<0.f || pchan->limitmax[0]>0.f))
890
if (!(pchan->ikflag & BONE_IK_NO_XDOF) && !(pchan->ikflag & BONE_IK_NO_XDOF_TEMP) &&
891
(!(pchan->ikflag & BONE_IK_XLIMIT) || pchan->limitmin[0] < 0.f || pchan->limitmax[0] > 0.f))
887
895
if (!(pchan->ikflag & BONE_IK_NO_YDOF) && !(pchan->ikflag & BONE_IK_NO_YDOF_TEMP) &&
888
(!(pchan->ikflag & BONE_IK_YLIMIT) || pchan->limitmin[1]<0.f || pchan->limitmax[1]>0.f))
896
(!(pchan->ikflag & BONE_IK_YLIMIT) || pchan->limitmin[1] < 0.f || pchan->limitmax[1] > 0.f))
890
900
if (!(pchan->ikflag & BONE_IK_NO_ZDOF) && !(pchan->ikflag & BONE_IK_NO_ZDOF_TEMP) &&
891
(!(pchan->ikflag & BONE_IK_ZLIMIT) || pchan->limitmin[2]<0.f || pchan->limitmax[2]>0.f))
901
(!(pchan->ikflag & BONE_IK_ZLIMIT) || pchan->limitmin[2] < 0.f || pchan->limitmax[2] > 0.f))
894
906
if (tree->stretch && (pchan->ikstretch > 0.0)) {
895
907
flag |= IK_TRANSY;
921
933
* bone length is computed from bone->length multiplied by the scaling factor of
922
934
* the armature. Non-uniform scaling will give bad result!
924
switch (flag & (IK_XDOF|IK_YDOF|IK_ZDOF))
927
ikchan->jointType = 0;
931
// RX only, get the X rotation
932
ikchan->jointType = IK_XDOF;
936
// RY only, get the Y rotation
937
ikchan->jointType = IK_YDOF;
941
// RZ only, get the Zz rotation
942
ikchan->jointType = IK_ZDOF;
945
case IK_XDOF|IK_YDOF:
946
ikchan->jointType = IK_XDOF|IK_YDOF;
949
case IK_XDOF|IK_ZDOF:
951
ikchan->jointType = IK_SWING;
954
case IK_YDOF|IK_ZDOF:
956
ikchan->jointType = IK_ZDOF|IK_YDOF;
959
case IK_XDOF|IK_YDOF|IK_ZDOF:
961
if (pchan->ikflag & (BONE_IK_XLIMIT|BONE_IK_YLIMIT|BONE_IK_ZLIMIT))
962
// decompose in a Swing+RotY joint
963
ikchan->jointType = IK_SWING|IK_YDOF;
965
ikchan->jointType = IK_REVOLUTE;
936
switch (flag & (IK_XDOF | IK_YDOF | IK_ZDOF)) {
938
ikchan->jointType = 0;
942
// RX only, get the X rotation
943
ikchan->jointType = IK_XDOF;
947
// RY only, get the Y rotation
948
ikchan->jointType = IK_YDOF;
952
// RZ only, get the Zz rotation
953
ikchan->jointType = IK_ZDOF;
956
case IK_XDOF | IK_YDOF:
957
ikchan->jointType = IK_XDOF | IK_YDOF;
960
case IK_XDOF | IK_ZDOF:
962
ikchan->jointType = IK_SWING;
965
case IK_YDOF | IK_ZDOF:
967
ikchan->jointType = IK_ZDOF | IK_YDOF;
970
case IK_XDOF | IK_YDOF | IK_ZDOF:
972
if (pchan->ikflag & (BONE_IK_XLIMIT | BONE_IK_YLIMIT | BONE_IK_ZLIMIT))
973
// decompose in a Swing+RotY joint
974
ikchan->jointType = IK_SWING | IK_YDOF;
976
ikchan->jointType = IK_REVOLUTE;
969
980
if (flag & IK_TRANSY) {
970
981
ikchan->jointType |= IK_TRANSY;
1132
1145
// in Blender, the rest pose is always 0 for joints
1146
BKE_pose_rest(ikscene);
1135
rot = &ikscene->jointArray(0);
1136
for (a=0, ikchan = ikscene->channels; a<tree->totchannel; ++a, ++ikchan) {
1137
pchan= ikchan->pchan;
1148
rot = ikscene->jointArray(0);
1150
for (a = 0, ikchan = ikscene->channels; a < tree->totchannel; ++a, ++ikchan) {
1151
pchan = ikchan->pchan;
1140
1154
KDL::Frame tip(iTaSC::F_identity);
1155
// compute the position and rotation of the head from previous segment
1141
1156
Vector3 *fl = bone->bone_mat;
1142
1157
KDL::Rotation brot(
1143
fl[0][0], fl[1][0], fl[2][0],
1144
fl[0][1], fl[1][1], fl[2][1],
1145
fl[0][2], fl[1][2], fl[2][2]);
1146
KDL::Vector bpos(bone->head[0], bone->head[1], bone->head[2]);
1158
fl[0][0], fl[1][0], fl[2][0],
1159
fl[0][1], fl[1][1], fl[2][1],
1160
fl[0][2], fl[1][2], fl[2][2]);
1161
// if the bone is disconnected, the head is movable in pose mode
1162
// take that into account by using pose matrix instead of bone
1163
// Note that pose is expressed in armature space, convert to previous bone space
1165
float R_parmat[3][3];
1166
float iR_parmat[3][3];
1168
copy_m3_m4(R_parmat, pchan->parent->pose_mat);
1172
sub_v3_v3v3(start, pchan->pose_head, pchan->parent->pose_tail);
1174
start[0] = start[1] = start[2] = 0.0f;
1175
invert_m3_m3(iR_parmat, R_parmat);
1176
normalize_m3(iR_parmat);
1177
mul_m3_v3(iR_parmat, start);
1179
KDL::Vector bpos(start[0], start[1], start[2]);
1180
bpos *= ikscene->blScale;
1148
1181
KDL::Frame head(brot, bpos);
1150
1183
// rest pose length of the bone taking scaling into account
1151
length= bone->length*scale;
1184
length = bone->length * ikscene->blScale;
1152
1185
parent = (a > 0) ? ikscene->channels[tree->parent[a]].tail : root;
1153
1186
// first the fixed segment to the bone head
1154
if (head.p.Norm() > KDL::epsilon || head.M.GetRot().Norm() > KDL::epsilon) {
1187
if (!(ikchan->pchan->bone->flag & BONE_CONNECTED) || head.M.GetRot().Norm() > KDL::epsilon) {
1155
1188
joint = bone->name;
1157
1190
ret = arm->addSegment(joint, parent, KDL::Joint::None, 0.0, head);
1162
1195
tip.p[1] = length;
1164
1197
joint = bone->name;
1165
weight[0] = (1.0-pchan->stiffness[0]);
1166
weight[1] = (1.0-pchan->stiffness[1]);
1167
weight[2] = (1.0-pchan->stiffness[2]);
1168
switch (ikchan->jointType & ~IK_TRANSY)
1172
if (!(ikchan->jointType & IK_TRANSY)) {
1198
weight[0] = (1.0 - pchan->stiffness[0]);
1199
weight[1] = (1.0 - pchan->stiffness[1]);
1200
weight[2] = (1.0 - pchan->stiffness[2]);
1201
switch (ikchan->jointType & ~IK_TRANSY) {
1174
1205
ret = arm->addSegment(joint, parent, KDL::Joint::None, 0.0, tip);
1178
// RX only, get the X rotation
1180
ret = arm->addSegment(joint, parent, KDL::Joint::RotX, rot[0], tip);
1181
weights.push_back(weight[0]);
1184
// RY only, get the Y rotation
1186
ret = arm->addSegment(joint, parent, KDL::Joint::RotY, rot[0], tip);
1187
weights.push_back(weight[1]);
1190
// RZ only, get the Zz rotation
1192
ret = arm->addSegment(joint, parent, KDL::Joint::RotZ, rot[0], tip);
1193
weights.push_back(weight[2]);
1195
case IK_XDOF|IK_YDOF:
1197
ret = arm->addSegment(joint, parent, KDL::Joint::RotX, rot[0]);
1198
weights.push_back(weight[0]);
1203
ret = arm->addSegment(joint, parent, KDL::Joint::RotY, rot[1], tip);
1204
weights.push_back(weight[1]);
1209
ret = arm->addSegment(joint, parent, KDL::Joint::Swing, rot[0], tip);
1210
weights.push_back(weight[0]);
1211
weights.push_back(weight[2]);
1213
case IK_YDOF|IK_ZDOF:
1216
ret = arm->addSegment(joint, parent, KDL::Joint::RotZ, rot[0]);
1217
weights.push_back(weight[2]);
1222
ret = arm->addSegment(joint, parent, KDL::Joint::RotY, rot[1], tip);
1223
weights.push_back(weight[1]);
1226
case IK_SWING|IK_YDOF:
1227
// decompose in a Swing+RotY joint
1229
ret = arm->addSegment(joint, parent, KDL::Joint::Swing, rot[0]);
1230
weights.push_back(weight[0]);
1231
weights.push_back(weight[2]);
1236
ret = arm->addSegment(joint, parent, KDL::Joint::RotY, rot[2], tip);
1237
weights.push_back(weight[1]);
1242
ret = arm->addSegment(joint, parent, KDL::Joint::Sphere, rot[0], tip);
1243
weights.push_back(weight[0]);
1244
weights.push_back(weight[1]);
1245
weights.push_back(weight[2]);
1208
// RX only, get the X rotation
1210
ret = arm->addSegment(joint, parent, KDL::Joint::RotX, rot[0], tip);
1211
weights.push_back(weight[0]);
1214
// RY only, get the Y rotation
1216
ret = arm->addSegment(joint, parent, KDL::Joint::RotY, rot[0], tip);
1217
weights.push_back(weight[1]);
1220
// RZ only, get the Zz rotation
1222
ret = arm->addSegment(joint, parent, KDL::Joint::RotZ, rot[0], tip);
1223
weights.push_back(weight[2]);
1225
case IK_XDOF | IK_YDOF:
1227
ret = arm->addSegment(joint, parent, KDL::Joint::RotX, rot[0]);
1228
weights.push_back(weight[0]);
1233
ret = arm->addSegment(joint, parent, KDL::Joint::RotY, rot[1], tip);
1234
weights.push_back(weight[1]);
1239
ret = arm->addSegment(joint, parent, KDL::Joint::Swing, rot[0], tip);
1240
weights.push_back(weight[0]);
1241
weights.push_back(weight[2]);
1243
case IK_YDOF | IK_ZDOF:
1246
ret = arm->addSegment(joint, parent, KDL::Joint::RotZ, rot[0]);
1247
weights.push_back(weight[2]);
1252
ret = arm->addSegment(joint, parent, KDL::Joint::RotY, rot[1], tip);
1253
weights.push_back(weight[1]);
1256
case IK_SWING | IK_YDOF:
1257
// decompose in a Swing+RotY joint
1259
ret = arm->addSegment(joint, parent, KDL::Joint::Swing, rot[0]);
1260
weights.push_back(weight[0]);
1261
weights.push_back(weight[2]);
1266
ret = arm->addSegment(joint, parent, KDL::Joint::RotY, rot[2], tip);
1267
weights.push_back(weight[1]);
1272
ret = arm->addSegment(joint, parent, KDL::Joint::Sphere, rot[0], tip);
1273
weights.push_back(weight[0]);
1274
weights.push_back(weight[1]);
1275
weights.push_back(weight[2]);
1248
1278
if (ret && (ikchan->jointType & IK_TRANSY)) {
1249
1279
parent = joint;
1250
1280
joint = bone->name;
1251
1281
joint += ":TY";
1252
ret = arm->addSegment(joint, parent, KDL::Joint::TransY, rot[ikchan->ndof-1]);
1253
float ikstretch = pchan->ikstretch*pchan->ikstretch;
1254
weight[1] = (1.0-MIN2(1.0-ikstretch, 0.99));
1282
ret = arm->addSegment(joint, parent, KDL::Joint::TransY, rot[ikchan->ndof - 1]);
1283
const float ikstretch = pchan->ikstretch * pchan->ikstretch;
1284
/* why invert twice here? */
1285
weight[1] = (1.0 - min_ff(1.0 - ikstretch, 1.0f - 0.001f));
1255
1286
weights.push_back(weight[1]);
1438
1469
switch (condata->type) {
1439
case CONSTRAINT_IK_COPYPOSE:
1441
if (condata->flag & CONSTRAINT_IK_ROT) {
1442
if (!(condata->flag & CONSTRAINT_IK_NO_ROT_X))
1443
controltype |= iTaSC::CopyPose::CTL_ROTATIONX;
1444
if (!(condata->flag & CONSTRAINT_IK_NO_ROT_Y))
1445
controltype |= iTaSC::CopyPose::CTL_ROTATIONY;
1446
if (!(condata->flag & CONSTRAINT_IK_NO_ROT_Z))
1447
controltype |= iTaSC::CopyPose::CTL_ROTATIONZ;
1449
if (condata->flag & CONSTRAINT_IK_POS) {
1450
if (!(condata->flag & CONSTRAINT_IK_NO_POS_X))
1451
controltype |= iTaSC::CopyPose::CTL_POSITIONX;
1452
if (!(condata->flag & CONSTRAINT_IK_NO_POS_Y))
1453
controltype |= iTaSC::CopyPose::CTL_POSITIONY;
1454
if (!(condata->flag & CONSTRAINT_IK_NO_POS_Z))
1455
controltype |= iTaSC::CopyPose::CTL_POSITIONZ;
1458
iktarget->constraint = new iTaSC::CopyPose(controltype, controltype, bonelen);
1460
if (controltype & iTaSC::CopyPose::CTL_POSITION)
1461
iktarget->constraint->setControlParameter(iTaSC::CopyPose::ID_POSITION, iTaSC::ACT_ALPHA, condata->weight);
1462
if (controltype & iTaSC::CopyPose::CTL_ROTATION)
1463
iktarget->constraint->setControlParameter(iTaSC::CopyPose::ID_ROTATION, iTaSC::ACT_ALPHA, condata->orientweight);
1464
iktarget->constraint->registerCallback(copypose_callback, iktarget);
1465
iktarget->errorCallback = copypose_error;
1466
iktarget->controlType = controltype;
1470
case CONSTRAINT_IK_COPYPOSE:
1472
if (condata->flag & CONSTRAINT_IK_ROT) {
1473
if (!(condata->flag & CONSTRAINT_IK_NO_ROT_X))
1474
controltype |= iTaSC::CopyPose::CTL_ROTATIONX;
1475
if (!(condata->flag & CONSTRAINT_IK_NO_ROT_Y))
1476
controltype |= iTaSC::CopyPose::CTL_ROTATIONY;
1477
if (!(condata->flag & CONSTRAINT_IK_NO_ROT_Z))
1478
controltype |= iTaSC::CopyPose::CTL_ROTATIONZ;
1480
if (condata->flag & CONSTRAINT_IK_POS) {
1481
if (!(condata->flag & CONSTRAINT_IK_NO_POS_X))
1482
controltype |= iTaSC::CopyPose::CTL_POSITIONX;
1483
if (!(condata->flag & CONSTRAINT_IK_NO_POS_Y))
1484
controltype |= iTaSC::CopyPose::CTL_POSITIONY;
1485
if (!(condata->flag & CONSTRAINT_IK_NO_POS_Z))
1486
controltype |= iTaSC::CopyPose::CTL_POSITIONZ;
1489
iktarget->constraint = new iTaSC::CopyPose(controltype, controltype, bonelen);
1491
if (controltype & iTaSC::CopyPose::CTL_POSITION)
1492
iktarget->constraint->setControlParameter(iTaSC::CopyPose::ID_POSITION, iTaSC::ACT_ALPHA, condata->weight);
1493
if (controltype & iTaSC::CopyPose::CTL_ROTATION)
1494
iktarget->constraint->setControlParameter(iTaSC::CopyPose::ID_ROTATION, iTaSC::ACT_ALPHA, condata->orientweight);
1495
iktarget->constraint->registerCallback(copypose_callback, iktarget);
1496
iktarget->errorCallback = copypose_error;
1497
iktarget->controlType = controltype;
1498
// add the constraint
1499
if (condata->flag & CONSTRAINT_IK_TARGETAXIS)
1500
ret = scene->addConstraintSet(iktarget->constraintName, iktarget->constraint, iktarget->targetName, armname, "", ikscene->channels[iktarget->channel].tail);
1502
ret = scene->addConstraintSet(iktarget->constraintName, iktarget->constraint, armname, iktarget->targetName, ikscene->channels[iktarget->channel].tail);
1505
case CONSTRAINT_IK_DISTANCE:
1506
iktarget->constraint = new iTaSC::Distance(bonelen);
1507
iktarget->constraint->setControlParameter(iTaSC::Distance::ID_DISTANCE, iTaSC::ACT_VALUE, condata->dist);
1508
iktarget->constraint->registerCallback(distance_callback, iktarget);
1509
iktarget->errorCallback = distance_error;
1510
// we can update the weight on each substep
1511
iktarget->constraint->substep(true);
1467
1512
// add the constraint
1468
if (condata->flag & CONSTRAINT_IK_TARGETAXIS)
1469
ret = scene->addConstraintSet(iktarget->constraintName, iktarget->constraint, iktarget->targetName, armname, "", ikscene->channels[iktarget->channel].tail);
1471
ret = scene->addConstraintSet(iktarget->constraintName, iktarget->constraint, armname, iktarget->targetName, ikscene->channels[iktarget->channel].tail);
1474
case CONSTRAINT_IK_DISTANCE:
1475
iktarget->constraint = new iTaSC::Distance(bonelen);
1476
iktarget->constraint->setControlParameter(iTaSC::Distance::ID_DISTANCE, iTaSC::ACT_VALUE, condata->dist);
1477
iktarget->constraint->registerCallback(distance_callback, iktarget);
1478
iktarget->errorCallback = distance_error;
1479
// we can update the weight on each substep
1480
iktarget->constraint->substep(true);
1481
// add the constraint
1482
ret = scene->addConstraintSet(iktarget->constraintName, iktarget->constraint, armname, iktarget->targetName, ikscene->channels[iktarget->channel].tail);
1513
ret = scene->addConstraintSet(iktarget->constraintName, iktarget->constraint, armname, iktarget->targetName, ikscene->channels[iktarget->channel].tail);
1489
!scene->addCache(ikscene->cache) ||
1490
!scene->addSolver(ikscene->solver) ||
1491
!scene->initialize()) {
1520
!scene->addCache(ikscene->cache) ||
1521
!scene->addSolver(ikscene->solver) ||
1522
!scene->initialize()) {
1492
1523
delete ikscene;
1493
1524
ikscene = NULL;
1495
1526
return ikscene;
1498
static void create_scene(Scene *scene, Object *ob)
1529
static void create_scene(Scene *scene, Object *ob, float ctime)
1500
1531
bPoseChannel *pchan;
1502
1533
// create the IK scene
1503
for (pchan= (bPoseChannel *)ob->pose->chanbase.first; pchan; pchan= (bPoseChannel *)pchan->next) {
1534
for (pchan = (bPoseChannel *)ob->pose->chanbase.first; pchan; pchan = (bPoseChannel *)pchan->next) {
1504
1535
// by construction there is only one tree
1505
PoseTree *tree = (PoseTree*)pchan->iktree.first;
1536
PoseTree *tree = (PoseTree *)pchan->iktree.first;
1507
IK_Data* ikdata = get_ikdata(ob->pose);
1538
IK_Data *ikdata = get_ikdata(ob->pose);
1508
1539
// convert tree in iTaSC::Scene
1509
IK_Scene* ikscene = convert_tree(scene, ob, pchan);
1540
IK_Scene *ikscene = convert_tree(scene, ob, pchan, ctime);
1511
1542
ikscene->next = ikdata->first;
1512
1543
ikdata->first = ikscene;
1514
1545
// delete the trees once we are done
1516
1547
BLI_remlink(&pchan->iktree, tree);
1517
1548
BLI_freelistN(&tree->targets);
1518
1549
if (tree->pchan) MEM_freeN(tree->pchan);
1519
1550
if (tree->parent) MEM_freeN(tree->parent);
1520
1551
if (tree->basis_change) MEM_freeN(tree->basis_change);
1521
1552
MEM_freeN(tree);
1522
tree = (PoseTree*)pchan->iktree.first;
1553
tree = (PoseTree *)pchan->iktree.first;
1528
static void init_scene(Object *ob)
1559
/* returns 1 if scaling has changed and tree must be reinitialized */
1560
static int init_scene(Object *ob)
1562
// check also if scaling has changed
1563
float scale = len_v3(ob->obmat[1]);
1530
1566
if (ob->pose->ikdata) {
1531
for (IK_Scene* scene = ((IK_Data*)ob->pose->ikdata)->first;
1533
scene = scene->next) {
1567
for (scene = ((IK_Data *)ob->pose->ikdata)->first;
1569
scene = scene->next) {
1570
if (fabs(scene->blScale - scale) > KDL::epsilon)
1534
1572
scene->channels[0].pchan->flag |= POSE_IKTREE;
1539
static void execute_scene(Scene* blscene, IK_Scene* ikscene, bItasc* ikparam, float ctime, float frtime)
1578
static void execute_scene(Scene *blscene, IK_Scene *ikscene, bItasc *ikparam, float ctime, float frtime)
1543
1582
if (ikparam->flag & ITASC_SIMULATION) {
1544
for (i=0, ikchan=ikscene->channels; i<ikscene->numchan; i++, ++ikchan) {
1583
for (i = 0, ikchan = ikscene->channels; i < ikscene->numchan; i++, ++ikchan) {
1545
1584
// In simulation mode we don't allow external contraint to change our bones, mark the channel done
1546
// also tell Blender that this channel is part of IK tree (cleared on each where_is_pose()
1547
ikchan->pchan->flag |= (POSE_DONE|POSE_CHAIN);
1585
// also tell Blender that this channel is part of IK tree (cleared on each BKE_pose_where_is()
1586
ikchan->pchan->flag |= (POSE_DONE | POSE_CHAIN);
1548
1587
ikchan->jointValid = 0;
1552
1591
// in animation mode, we must get the bone position from action and constraints
1553
for (i=0, ikchan=ikscene->channels; i<ikscene->numchan; i++, ++ikchan) {
1592
for (i = 0, ikchan = ikscene->channels; i < ikscene->numchan; i++, ++ikchan) {
1554
1593
if (!(ikchan->pchan->flag & POSE_DONE))
1555
where_is_pose_bone(blscene, ikscene->blArmature, ikchan->pchan, ctime, 1);
1556
// tell blender that this channel was controlled by IK, it's cleared on each where_is_pose()
1557
ikchan->pchan->flag |= (POSE_DONE|POSE_CHAIN);
1594
BKE_pose_where_is_bone(blscene, ikscene->blArmature, ikchan->pchan, ctime, 1);
1595
// tell blender that this channel was controlled by IK, it's cleared on each BKE_pose_where_is()
1596
ikchan->pchan->flag |= (POSE_DONE | POSE_CHAIN);
1558
1597
ikchan->jointValid = 0;
1561
1600
// only run execute the scene if at least one of our target is enabled
1562
for (i=ikscene->targets.size(); i > 0; --i) {
1563
IK_Target* iktarget = ikscene->targets[i-1];
1601
for (i = ikscene->targets.size(); i > 0; --i) {
1602
IK_Target *iktarget = ikscene->targets[i - 1];
1564
1603
if (!(iktarget->blenderConstraint->flag & CONSTRAINT_OFF))
1706
1751
if (ob->pose->ikdata != NULL && !(ob->pose->flag & POSE_WAS_REBUILT)) {
1752
if (!init_scene(ob))
1710
1755
// first remove old scene
1711
1756
itasc_clear_data(ob->pose);
1712
1757
// we should handle all the constraint and mark them all disabled
1713
1758
// for blender but we'll start with the IK constraint alone
1714
for (pchan= (bPoseChannel *)ob->pose->chanbase.first; pchan; pchan= (bPoseChannel *)pchan->next) {
1759
for (pchan = (bPoseChannel *)ob->pose->chanbase.first; pchan; pchan = (bPoseChannel *)pchan->next) {
1715
1760
if (pchan->constflag & PCHAN_HAS_IK)
1716
1761
count += initialize_scene(ob, pchan);
1718
// if at least one tree, create the scenes from the PoseTree stored in the channels
1720
create_scene(scene, ob);
1721
itasc_update_param(ob->pose);
1763
// if at least one tree, create the scenes from the PoseTree stored in the channels
1764
// postpone until execute_tree: this way the pose constraint are included
1766
// create_scene(scene, ob, ctime);
1767
//itasc_update_param(ob->pose);
1722
1768
// make sure we don't rebuilt until the user changes something important
1723
1769
ob->pose->flag &= ~POSE_WAS_REBUILT;
1726
1772
void itasc_execute_tree(struct Scene *scene, struct Object *ob, struct bPoseChannel *pchan, float ctime)
1774
if (!ob->pose->ikdata) {
1775
// IK tree not yet created, no it now
1776
create_scene(scene, ob, ctime);
1777
itasc_update_param(ob->pose);
1728
1779
if (ob->pose->ikdata) {
1729
IK_Data* ikdata = (IK_Data*)ob->pose->ikdata;
1730
bItasc* ikparam = (bItasc*) ob->pose->ikparam;
1780
IK_Data *ikdata = (IK_Data *)ob->pose->ikdata;
1781
bItasc *ikparam = (bItasc *) ob->pose->ikparam;
1731
1782
// we need default parameters
1732
1783
if (!ikparam) ikparam = &DefIKParam;
1734
for (IK_Scene* ikscene = ikdata->first; ikscene; ikscene = ikscene->next) {
1785
for (IK_Scene *ikscene = ikdata->first; ikscene; ikscene = ikscene->next) {
1735
1786
if (ikscene->channels[0].pchan == pchan) {
1736
float timestep = scene->r.frs_sec_base/scene->r.frs_sec;
1787
float timestep = scene->r.frs_sec_base / scene->r.frs_sec;
1737
1788
if (ob->pose->flag & POSE_GAME_ENGINE) {
1738
1789
timestep = ob->pose->ctime;
1739
1790
// limit the timestep to avoid excessive number of iteration