beginning of IK test

ikSolver.cpp

@@ -0,0 +1,60 @@
+#include "ikSolver.h"
+#include "bone.h"
+
+namespace vb01{
+	void IkSolver::calculateFabrik(int chainLength, Bone *boneChain[], Vector3 boneIkPos[], Vector3 targetPos){
+		float sumLengths = 0;
+		for(int i = 0; i < chainLength; i++)
+			sumLengths += boneChain[i]->getLength();
+		
+		Vector3 startPos = boneChain[chainLength - 1]->getModelSpacePos();
+		if(startPos.getDistanceFrom(targetPos) < sumLengths){
+			int numIterations = 500;
+	
+			for(int i = 0 ; i < numIterations; i++){
+				bool backward = (i % 2 == 0);
+
+				for(int j = 0; j < chainLength; j++){
+					int boneId;
+					float length;
+					Vector3 ikPos, bonePos, fromBoneToIkPos;
+					if(backward){
+						boneId = j;
+						length = boneChain[boneId]->getLength();
+						ikPos = (j == 0 ? targetPos : boneIkPos[boneId - 1]);
+					}
+					else{
+						boneId = chainLength - 1 - j;
+						if(j == 0){
+							ikPos = startPos;
+							length = 0;
+						}
+						else{
+							ikPos = boneIkPos[boneId + 1];
+							length = boneChain[boneId + 0]->getLength();
+						}
+					}
+					bonePos = boneIkPos[boneId];
+					fromBoneToIkPos = (ikPos - bonePos).norm();
+					boneIkPos[boneId] = ikPos - fromBoneToIkPos * length;
+				}
+			}
+		}
+		else{
+			Vector3 startToEndVec = (targetPos - startPos).norm();
+			for(int i = chainLength - 1; i >= 0; i--){
+				float length;
+				Vector3 bonePos;
+				if(i == chainLength - 1){
+					length = 0;
+					bonePos = boneIkPos[i];
+				}
+				else{
+					length = boneChain[i]->getLength();
+					bonePos = boneIkPos[i + 1];
+				}
+				boneIkPos[i] = bonePos + startToEndVec * length;
+			}
+		}
+	}
+}