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