D3DXMATRIX effTrans, destTrans, curBoneTrans;
D3DXVECTOR3 effPos, destPos, curBonePos;
D3DXVECTOR3 curBoneToEff, curBoneToDest;
D3DXVECTOR3 cross, refAxis;
D3DXVECTOR4 boneToEff, boneToDest;
D3DXQUATERNION rot, origEffRot, tmpRotQ;
D3DXMATRIX tmpRot, curBoneTransInv;
destTrans = _destBone->getCombinedTrans();
destPos = D3DXVECTOR3(destTrans(3,0), destTrans(3,1), destTrans(3,2));
origEffRot = _effBone->getRot();
double dot, angle, distance2;
float x;
for (int i=0;i<_numBone;i++)
{
_boneList[i]->UpdateIK()
}
_effBone->UpdateIK();
for (int i=0;i<_iteration;i++)
{
for (int b=0;b<_numBone;b++)
{
//get end effector position
effTrans = _effBone->getCombinedTrans();
effPos = D3DXVECTOR3(effTrans(3,0), effTrans(3,1), effTrans(3,2));
//get current bone position
curBoneTrans = _boneList[b]->getCombinedTrans();
curBonePos = D3DXVECTOR3(curBoneTrans(3,0), curBoneTrans(3,1), curBoneTrans(3,2));
//create inverse of bone position
D3DXMatrixInverse(&curBoneTransInv, NULL, &curBoneTrans);
//create point-to-point vector from bone inverse matrix
D3DXVec3Transform(&boneToEff, &effPos, &curBoneTransInv);
D3DXVec3Transform(&boneToDest, &destPos, &curBoneTransInv);
//transfer the vectors over
curBoneToEff = (D3DXVECTOR3)boneToEff;
curBoneToDest = (D3DXVECTOR3)boneToDest;
//get distance between direction vectors
distance2 = D3DXVec3Length(&(curBoneToDest-curBoneToEff));
distance2 *= distance2;
if (distance2 < IK_MIN_DIST)
{
//stop if end effector is close enough
i = _iteration;
break;
}
D3DXVec3Normalize(&curBoneToEff, &curBoneToEff);
D3DXVec3Normalize(&curBoneToDest, &curBoneToDest);
//get dot product and angle between the 2 vectors
dot = D3DXVec3Dot(&curBoneToDest, &curBoneToEff);
if (dot > 1.0f)
{
continue;
}
angle = acos(dot);
if (fabs(angle) < IK_MIN_ANGLE)
{
continue;
}
if (angle < -_angleConstraint)
{
angle = -_angleConstraint;
}
else if (angle > _angleConstraint)
{
angle = _angleConstraint;
}
//get the cross product (rotation axis)
D3DXVec3Cross(&cross, &curBoneToEff, &curBoneToDest);
//get length of axis
distance2 = D3DXVec3Length(&cross);
distance2 *= distance2;
if (distance2 < IK_MIN_AXIS && i > 0)
{
//axis is too small (direction dest to target is too close) skip to next bone
continue;
}
D3DXVec3Normalize(&cross, &cross);
//build rotation from axis and angle
D3DXQuaternionRotationAxis(&rot, &cross, (float)angle);
//check is current bone is limited to x
if (_boneList[b]->getIsLimitX())
{
quatToEuler(rot, NULL, NULL, &x);
if (x < -PI)
{
x = -PI;
}
if (-IK_MIN_ROT_SUM < x)
{
x = -IK_MIN_ROT_SUM;
}
D3DXMatrixRotationX(&tmpRot, x);
D3DXQuaternionRotationMatrix(&rot, &tmpRot);
}
D3DXQuaternionNormalize(&rot, &rot);
//apply rotation to bone
//combine with previous rotation for multiple iterations
tmpRotQ = _boneList[b]->getRot();
tmpRotQ *= rot;
D3DXQuaternionNormalize(&rot, &rot);
_boneList[b]->setRot(tmpRotQ);
for (int j=b;j>=0;j--)
{
_boneList[j]->UpdateFromIK();
}
_effBone->UpdateFromIK();
}
}
_effBone->setRot(origEffRot);
_effBone->UpdateFromIK();