This week only. Pastebin PRO Accounts Christmas Special! Don't miss out!Want more features on Pastebin? Sign Up, it's FREE!
Guest

ik solver

By: a guest on Aug 27th, 2012  |  syntax: C++  |  size: 3.39 KB  |  views: 15  |  expires: Never
download  |  raw  |  embed  |  report abuse  |  print
Text below is selected. Please press Ctrl+C to copy to your clipboard. (⌘+C on Mac)
  1. D3DXMATRIX effTrans, destTrans, curBoneTrans;
  2.         D3DXVECTOR3 effPos, destPos, curBonePos;
  3.  
  4.         D3DXVECTOR3 curBoneToEff, curBoneToDest;
  5.         D3DXVECTOR3 cross, refAxis;
  6.  
  7.         D3DXVECTOR4 boneToEff, boneToDest;
  8.  
  9.         D3DXQUATERNION rot, origEffRot, tmpRotQ;
  10.  
  11.         D3DXMATRIX tmpRot, curBoneTransInv;
  12.  
  13.         destTrans = _destBone->getCombinedTrans();
  14.         destPos = D3DXVECTOR3(destTrans(3,0), destTrans(3,1), destTrans(3,2));
  15.  
  16.         origEffRot = _effBone->getRot();
  17.  
  18.         double dot, angle, distance2;
  19.         float x;
  20.  
  21.         for (int i=0;i<_numBone;i++)
  22.         {
  23.                 _boneList[i]->UpdateIK()
  24.         }
  25.         _effBone->UpdateIK();
  26.  
  27.         for (int i=0;i<_iteration;i++)
  28.         {
  29.                 for (int b=0;b<_numBone;b++)
  30.                 {
  31.                         //get end effector position
  32.                         effTrans = _effBone->getCombinedTrans();
  33.                         effPos = D3DXVECTOR3(effTrans(3,0), effTrans(3,1), effTrans(3,2));
  34.  
  35.                         //get current bone position
  36.                         curBoneTrans = _boneList[b]->getCombinedTrans();
  37.                         curBonePos = D3DXVECTOR3(curBoneTrans(3,0), curBoneTrans(3,1), curBoneTrans(3,2));
  38.  
  39.                         //create inverse of bone position
  40.                         D3DXMatrixInverse(&curBoneTransInv, NULL, &curBoneTrans);
  41.  
  42.  
  43.                         //create point-to-point vector from bone inverse matrix
  44.                         D3DXVec3Transform(&boneToEff, &effPos, &curBoneTransInv);
  45.                         D3DXVec3Transform(&boneToDest, &destPos, &curBoneTransInv);
  46.  
  47.                         //transfer the vectors over
  48.                         curBoneToEff = (D3DXVECTOR3)boneToEff;
  49.                         curBoneToDest = (D3DXVECTOR3)boneToDest;
  50.                        
  51.                        
  52.                         //get distance between direction vectors
  53.                         distance2 = D3DXVec3Length(&(curBoneToDest-curBoneToEff));
  54.                         distance2 *= distance2;
  55.                         if (distance2 < IK_MIN_DIST)
  56.                         {
  57.                                 //stop if end effector is close enough
  58.                                 i = _iteration;
  59.                                 break;
  60.                         }
  61.  
  62.                         D3DXVec3Normalize(&curBoneToEff, &curBoneToEff);
  63.                         D3DXVec3Normalize(&curBoneToDest, &curBoneToDest);
  64.  
  65.                         //get dot product and angle between the 2 vectors
  66.                         dot = D3DXVec3Dot(&curBoneToDest, &curBoneToEff);
  67.                         if (dot > 1.0f)
  68.                         {
  69.                                 continue;
  70.                         }
  71.                         angle = acos(dot);
  72.                         if (fabs(angle) < IK_MIN_ANGLE)
  73.                         {
  74.                                 continue;
  75.                         }
  76.                         if (angle < -_angleConstraint)
  77.                         {
  78.                                 angle = -_angleConstraint;
  79.                         }
  80.                         else if (angle > _angleConstraint)
  81.                         {
  82.                                 angle = _angleConstraint;
  83.                         }
  84.  
  85.                         //get the cross product (rotation axis)
  86.                         D3DXVec3Cross(&cross, &curBoneToEff, &curBoneToDest);
  87.  
  88.                         //get length of axis
  89.                         distance2 = D3DXVec3Length(&cross);
  90.                         distance2 *= distance2;
  91.                         if (distance2 < IK_MIN_AXIS && i > 0)
  92.                         {
  93.                                 //axis is too small (direction dest to target is too close) skip to next bone
  94.                                 continue;
  95.                         }
  96.  
  97.                         D3DXVec3Normalize(&cross, &cross);
  98.                         //build rotation from axis and angle
  99.                         D3DXQuaternionRotationAxis(&rot, &cross, (float)angle);
  100.  
  101.                         //check is current bone is limited to x
  102.                         if (_boneList[b]->getIsLimitX())
  103.                         {
  104.                                 quatToEuler(rot, NULL, NULL, &x);
  105.  
  106.                                 if (x < -PI)
  107.                                 {
  108.                                         x = -PI;
  109.                                 }
  110.                                 if (-IK_MIN_ROT_SUM < x)
  111.                                 {
  112.                                         x = -IK_MIN_ROT_SUM;
  113.                                 }                                      
  114.  
  115.                                 D3DXMatrixRotationX(&tmpRot, x);
  116.                                 D3DXQuaternionRotationMatrix(&rot, &tmpRot);
  117.                         }
  118.                                
  119.                                 D3DXQuaternionNormalize(&rot, &rot);
  120.  
  121.                                 //apply rotation to bone
  122.                                 //combine with previous rotation for multiple iterations
  123.                                 tmpRotQ = _boneList[b]->getRot();
  124.                                 tmpRotQ *= rot;
  125.                                 D3DXQuaternionNormalize(&rot, &rot);
  126.                                 _boneList[b]->setRot(tmpRotQ);
  127.  
  128.                         for (int j=b;j>=0;j--)
  129.                         {
  130.                                 _boneList[j]->UpdateFromIK();
  131.                         }
  132.                         _effBone->UpdateFromIK();
  133.                 }
  134.         }
  135.         _effBone->setRot(origEffRot);
  136.         _effBone->UpdateFromIK();
clone this paste RAW Paste Data