ik solver

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();