From 18fab525e82972ae0db017bc213aaefa8c1532ed Mon Sep 17 00:00:00 2001
From: devZoGok <nenkokeko@yandex.com>
Date: Thu, 11 Feb 2021 14:17:27 +0200
Subject: [PATCH] influence of additional bones for IK chains

---
 ikSolver.cpp |  2 +-
 ikSolver.h   |  2 +-
 skeleton.cpp | 45 ++++++++++++++++++++++++---------------------
 skeleton.h   |  3 ++-
 4 files changed, 28 insertions(+), 24 deletions(-)

diff --git a/ikSolver.cpp b/ikSolver.cpp
index 32b6447..e86abd7 100644
--- a/ikSolver.cpp
+++ b/ikSolver.cpp
@@ -2,7 +2,7 @@
 #include "bone.h"
 
 namespace vb01{
-	void IkSolver::calculateFabrik(int chainLength, Bone *boneChain[], Vector3 boneIkPos[], Vector3 targetPos){
+	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();
diff --git a/ikSolver.h b/ikSolver.h
index 886045d..ee978e0 100644
--- a/ikSolver.h
+++ b/ikSolver.h
@@ -10,7 +10,7 @@ namespace vb01{
 		public:
 			IkSolver();
 			~IkSolver(){}
-			static void calculateFabrik(int, Bone*[], Vector3[], Vector3);
+			static void calculateFabrik(int, Bone**, Vector3[], Vector3);
 		private:
 	};
 }
diff --git a/skeleton.cpp b/skeleton.cpp
index 95f945d..d64bb21 100644
--- a/skeleton.cpp
+++ b/skeleton.cpp
@@ -17,7 +17,6 @@ namespace vb01{
 
 	void Skeleton::update(){
 		for(Bone *b : bones){
-			//b->update();
 			if(b->getIkTarget())
 				solveIk(b);
 		}
@@ -25,41 +24,35 @@ namespace vb01{
 	}
 
 	void Skeleton::solveIk(Bone *ikBone){
+		const int chainLength = ikBone->getIkChainLength();
 		Bone *ikTarget = ikBone->getIkTarget();
 		Bone *subBase = (Bone*)ikTarget->getParent();
 
-		const int chainLength = ikBone->getIkChainLength();
+		Bone **boneChain = getIkBoneChain(ikBone);
 		Vector3 boneIkPos[chainLength];
-		Bone *boneChain[chainLength];
-		Bone *ikBoneAncestor = ikBone;
-
-		for(int i = 0; i < chainLength; i++){
-			boneChain[i] = ikBoneAncestor;
-			ikBoneAncestor = (Bone*)ikBoneAncestor->getParent();
-			boneChain[i]->setPosePos(Vector3::VEC_ZERO);
-			boneChain[i]->setPoseRot(Quaternion::QUAT_W);
-			boneChain[i]->setPoseScale(Vector3::VEC_IJK);
-		}
-		for(int i = 0; i < chainLength; i++)
+		for(int i = chainLength - 1; i >= 0; i--){
 			boneIkPos[i] = subBase->globalToLocalPosition(boneChain[i]->localToGlobalPosition(Vector3::VEC_ZERO));
+		}
 
 	   	Vector3 targetPos = subBase->globalToLocalPosition(ikTarget->localToGlobalPosition(Vector3::VEC_ZERO));
 		IkSolver::calculateFabrik(chainLength, boneChain, boneIkPos, targetPos);
 		transformIkChain(chainLength, boneChain, boneIkPos, ikTarget);
+
+		delete[] boneChain;
 	}
 
-	void Skeleton::transformIkChain(int chainLength, Bone *boneChain[], Vector3 boneIkPos[], Bone* ikTarget){
+	void Skeleton::transformIkChain(int chainLength, Bone **boneChain, Vector3 boneIkPos[], Bone* ikTarget){
 		Bone *subBase = (Bone*)ikTarget->getParent();
 		for(int i = chainLength - 1; i >= 0; i--){
-			Vector3 tailPosBoneSpace, tailRestPosBoneSpace;
-			if(i == 0){
+			boneChain[i]->setPoseRot(Quaternion::QUAT_W);
+
+			Vector3 tailPosBoneSpace;
+			if(i == 0)
 				tailPosBoneSpace = subBase->globalToLocalPosition(ikTarget->localToGlobalPosition(Vector3::VEC_ZERO));
-				tailRestPosBoneSpace = subBase->globalToLocalPosition(boneChain[i]->localToGlobalPosition(Vector3(0, boneChain[i]->getLength(), 0)));
-			}
-			else{
+			else
 				tailPosBoneSpace = boneIkPos[i - 1];
-				tailRestPosBoneSpace = subBase->globalToLocalPosition(boneChain[i - 1]->localToGlobalPosition(Vector3::VEC_ZERO));
-			}
+
+			Vector3 tailRestPosBoneSpace = subBase->globalToLocalPosition(boneChain[i]->localToGlobalPosition(Vector3(0, boneChain[i]->getLength(), 0)));
 			Vector3 headRestPosBoneSpace = subBase->globalToLocalPosition(boneChain[i]->localToGlobalPosition(Vector3::VEC_ZERO));
 			Vector3 headDir = (tailPosBoneSpace - headRestPosBoneSpace).norm();
 			Vector3 boneDir = (tailRestPosBoneSpace - headRestPosBoneSpace).norm();
@@ -93,4 +86,14 @@ namespace vb01{
 		return bone;
 	}
 
+	Bone** Skeleton::getIkBoneChain(Bone *ikBone){
+		int chainLength = ikBone->getIkChainLength();
+		Bone **chain = new Bone*[chainLength];
+		Bone *ikBoneAncestor = ikBone;
+		for(int i = 0; i < chainLength; i++){
+			chain[i] = ikBoneAncestor;
+			ikBoneAncestor = (Bone*)ikBoneAncestor->getParent();
+		}
+		return chain;
+	}
 }
diff --git a/skeleton.h b/skeleton.h
index a7ec2f8..0072088 100644
--- a/skeleton.h
+++ b/skeleton.h
@@ -23,7 +23,8 @@ namespace vb01{
 			inline int getNumBones(){return bones.size();}
 		private:
 			void solveIk(Bone*);
-			void transformIkChain(int, Bone*[], Vector3[], Bone*);
+			void transformIkChain(int, Bone**, Vector3[], Bone*);
+			Bone** getIkBoneChain(Bone*);
 
 			std::string name;
 			std::vector<Bone*> bones;