beginning of IK test

CMakeLists.txt

@@ -17,7 +17,7 @@ set(math quaternion.cpp vector.cpp matrix.cpp ray.cpp)
 set(render particleEmitter.cpp camera.cpp light.cpp material.cpp mesh.cpp model.cpp node.cpp quad.cpp box.cpp root.cpp shader.cpp texture.cpp)
 set(model modelReader.cpp assimpModelReader.cpp vbModelReader.cpp)
 set(anim animationController.cpp animationChannel.cpp animation.cpp)
-set(armature skeleton.cpp bone.cpp)
+set(armature skeleton.cpp bone.cpp ikSolver.cpp)
 set(library ${render} ${math} ${model} ${utils} ${gui} ${armature} ${anim})
 
 add_library(vb01 SHARED ${library})
@@ -27,7 +27,7 @@ target_link_libraries(vb01 assimp)
 target_link_libraries(vb01 freetype)
 
 set(renderTest nodeTest.cpp cameraTest.cpp shaderTest.cpp particleEmitterTest.cpp)
-set(armatureTest boneTest.cpp)
+set(armatureTest boneTest.cpp ikSolverTest.cpp)
 set(modelTest vbModelReaderTest.cpp)
 set(animationTest animationChannelTest.cpp)
 set(test ${library} ${renderTest} ${armatureTest} ${modelTest} ${animationTest})

bone.cpp

@@ -53,17 +53,14 @@ namespace vb01{
 	void Bone::setPoseRot(Quaternion r){
 		this->poseRot = r;
 
-		setOrientation(restRot);
+		Vector3 rotAxis = r.getAxis();
+		rotAxis =
+		   	parent->globalToLocalPosition(localToGlobalPosition(rotAxis)) - 
+		   	parent->globalToLocalPosition(localToGlobalPosition(Vector3::VEC_ZERO)); 
 
-		Quaternion parentSpacePoseRot = Quaternion::QUAT_W;
-		vector<Node*> ancestors = getAncestors();
-		for(int i = 0; i < ancestors.size(); i++)
-			parentSpacePoseRot = parentSpacePoseRot * ancestors[i]->getOrientation();
-		for(int i = ancestors.size() - 1; i > 0; i--)
-			parentSpacePoseRot = parentSpacePoseRot * ancestors[i]->getOrientation().conj();
-		parentSpacePoseRot = r * parentSpacePoseRot;
+		Quaternion parentSpacePoseRot = Quaternion(r.getAngle(), rotAxis);
 
-		setOrientation(parentSpacePoseRot);
+		setOrientation(parentSpacePoseRot * restRot);
 	}
 
 	void Bone::setPoseScale(Vector3 s){

bone.h

@@ -20,6 +20,8 @@ namespace vb01{
 			inline int getIkChainLength(){return ikChainLength;}
 			inline void setIkChainLength(int ikChainLength){this->ikChainLength = ikChainLength;}
 			inline float getLength(){return length;}
+			inline bool isIkFromTail(){return ikFromTail;}
+			inline void setIkFromTail(bool ikFromTail){this->ikFromTail = ikFromTail;}
 			inline void setSkeleton(Skeleton *sk){this->skeleton = sk;}
 			inline Vector3 getInitAxis(int i){return initAxis[i];}
 			inline std::string getName(){return name;}
@@ -33,6 +35,7 @@ namespace vb01{
 			float length;
 			Bone *ikTarget = nullptr;
 			int ikChainLength = -1;
+			bool ikFromTail = true;
 			Skeleton *skeleton = nullptr;
 			Vector3 initAxis[3], restPos, posePos = Vector3::VEC_ZERO, restScale, poseScale = Vector3::VEC_IJK;
 			Quaternion restRot, poseRot = Quaternion::QUAT_W;

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;
+			}
+		}
+	}
+}

ikSolver.h

@@ -0,0 +1,18 @@
+#ifndef IK_SOLVER_H
+#define IK_SOLVER_H
+
+#include "vector.h"
+
+namespace vb01{
+	class Bone;
+
+	class IkSolver{
+		public:
+			IkSolver();
+			~IkSolver(){}
+			static void calculateFabrik(int, Bone*[], Vector3[], Vector3);
+		private:
+	};
+}
+
+#endif

ikSolverTest.cpp

@@ -0,0 +1,69 @@
+#include "ikSolverTest.h"
+#include "ikSolver.h"
+#include "skeleton.h"
+
+namespace vb01{
+	void IkSolverTest::setUp(){
+		rootNode = Root::getSingleton()->getRootNode();
+
+		skeleton = new Skeleton("");
+
+		Bone *boneA = new Bone("A", 1, Vector3::VEC_ZERO, Quaternion::QUAT_W, Vector3::VEC_IJK);
+		Bone *boneB = new Bone("B", 1, Vector3::VEC_J, Quaternion::QUAT_W, Vector3::VEC_IJK);
+		Bone *boneC = new Bone("C", 1, Vector3::VEC_J, Quaternion::QUAT_W, Vector3::VEC_IJK);
+		Bone *boneD = new Bone("D", 1, Vector3::VEC_J, Quaternion::QUAT_W, Vector3::VEC_IJK);
+		ikBone = new Bone("ik", 1, Vector3(0, 4, 0), Quaternion::QUAT_W, Vector3::VEC_IJK);
+
+		skeleton->addBone(boneA, (Bone*)rootNode);
+		skeleton->addBone(boneB, boneA);
+		skeleton->addBone(boneC, boneB);
+		skeleton->addBone(boneD, boneC);
+		skeleton->addBone(ikBone, boneA);
+
+		boneA->lookAt(Vector3::VEC_K, Vector3::VEC_J);
+		boneB->lookAt(Vector3::VEC_K, Vector3::VEC_J);
+		boneC->lookAt(Vector3::VEC_K, Vector3::VEC_J);
+		boneD->lookAt(Vector3::VEC_K, Vector3::VEC_J);
+
+		boneD->setIkChainLength(chainLength);
+		boneD->setIkTarget(ikBone);
+
+		ikPos = new Vector3[chainLength];
+		boneChain = new Bone*[chainLength];
+
+		resetIkData();
+	}
+
+	void IkSolverTest::tearDown(){
+	}
+
+	void IkSolverTest::testSolveIk(){
+		Vector3 ikBonePos = Vector3(1, 1, 1) * 5 + Vector3(0, 1, 0);
+		resetIkData();
+		IkSolver::calculateFabrik(chainLength, boneChain, ikPos, ikBonePos);
+
+		float eps = .01;
+
+		for(int i = chainLength - 1; i >= 0; i--)
+			CPPUNIT_ASSERT(ikPos[i].getDistanceFrom(Vector3(0, 1, 0) + Vector3(1, 1, 1).norm() * (chainLength - 1 - i)) <= eps);
+
+		ikBonePos = Vector3(0, 1, 0) * 2 + Vector3(2, 0, 0);
+		resetIkData();
+		IkSolver::calculateFabrik(chainLength, boneChain, ikPos, ikBonePos);
+
+		CPPUNIT_ASSERT(ikPos[1].getDistanceFrom(Vector3(0.171451, 1.98516, 0)) <= eps);
+		CPPUNIT_ASSERT(ikPos[0].getDistanceFrom(Vector3(1.08232, 2.39756, 0)) <= eps);
+
+	}
+
+	void IkSolverTest::resetIkData(){
+		Bone *rootBone = skeleton->getRootBone();
+		Bone *ikBoneAncestor = skeleton->getBone("D");
+
+		for(int i = 0; i < chainLength; i++){
+			boneChain[i] = ikBoneAncestor;
+			ikBoneAncestor = (Bone*)ikBoneAncestor->getParent();
+			ikPos[i] = boneChain[i]->getModelSpacePos();
+		}
+	}
+}

ikSolverTest.h

@@ -0,0 +1,34 @@
+#ifndef IK_SOLVER_TEST_H
+#define IK_SOLVER_TEST_H
+
+#include <cppunit/TestFixture.h>
+#include <cppunit/extensions/HelperMacros.h>
+#include "bone.h"
+
+namespace vb01{
+	class Skeleton;
+	class Node;
+	class Bone;
+
+	class IkSolverTest : public CppUnit::TestFixture{
+		CPPUNIT_TEST_SUITE(IkSolverTest);
+		CPPUNIT_TEST(testSolveIk);
+		CPPUNIT_TEST_SUITE_END();
+		public:
+			IkSolverTest(){}
+			~IkSolverTest(){}
+			void setUp();
+			void tearDown();
+			void testSolveIk();
+		private:
+			Skeleton *skeleton = nullptr;
+			Node *rootNode = nullptr;
+			int chainLength = 3;
+			Vector3 *ikPos;
+			Bone *ikBone = nullptr, **boneChain;
+
+			void resetIkData();
+	};
+}
+
+#endif

main.cpp

@@ -10,6 +10,7 @@
 #include "animationChannelTest.h"
 #include "shaderTest.h"
 #include "particleEmitterTest.h"
+#include "ikSolverTest.h"
 
 using namespace CppUnit;
 using namespace vb01;
@@ -23,6 +24,7 @@ int main(){
 	runner.addTest(AnimationChannelTest::suite());
 	runner.addTest(ShaderTest::suite());
 	runner.addTest(ParticleEmitterTest::suite());
+	runner.addTest(IkSolverTest::suite());
 	runner.run();
 	return 0;
 }

skeleton.cpp

@@ -2,6 +2,7 @@
 #include "animation.h"
 #include "animationController.h"
 #include "box.h"
+#include "ikSolver.h"
 
 using namespace std;
 
@@ -36,66 +37,11 @@ namespace vb01{
 			boneIkPos[i] = boneChain[i]->getModelSpacePos();
 		}
 
-		calculateFabrik(chainLength, boneChain, boneIkPos, targetPos);
+		IkSolver::calculateFabrik(chainLength, boneChain, boneIkPos, targetPos);
 
 		transformIkChain(chainLength, boneChain, boneIkPos, targetPos);
 	}
 
-	void Skeleton::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;
-			}
-		}
-	}
-
 	void Skeleton::transformIkChain(int chainLength, Bone *boneChain[], Vector3 boneIkPos[], Vector3 targetPos){
 		Node *rootBone = getRootBone()->getParent();
 		float boneAngles[chainLength];

skeleton.h

@@ -23,7 +23,6 @@ namespace vb01{
 			inline int getNumBones(){return bones.size();}
 		private:
 			void solveIk(Bone*);
-			void calculateFabrik(int, Bone*[], Vector3[], Vector3);
 			void transformIkChain(int, Bone*[], Vector3[], Vector3);
 
 			std::string name;