Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- 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();
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement