From 9eb87999cf03472f7e9cca7b9c6c4f746cbc91af Mon Sep 17 00:00:00 2001
From: devZoGok <nenkokeko@yandex.com>
Date: Sun, 6 Sep 2020 15:42:38 +0300
Subject: [PATCH] limited IK calculation

---
 animationController.cpp |  8 ++++-
 quaternion.h            |  4 +--
 skeleton.cpp            | 72 +++++++++++++++++++++++++++++++++--------
 vbModelReader.cpp       |  4 +++
 4 files changed, 71 insertions(+), 17 deletions(-)

diff --git a/animationController.cpp b/animationController.cpp
index eef435c..8c8a11d 100644
--- a/animationController.cpp
+++ b/animationController.cpp
@@ -99,9 +99,15 @@ namespace vb01{
 							break;
 					}
 				}
+				/*
+				swap(currentPos.y, currentPos.z);
+				currentPos.z = -currentPos.z;
+				swap(currentRot.y, currentRot.z);
+				currentRot.z = -currentRot.z;
+				*/
 
 				channelBone->setPosePos(currentPos);
-				//channelBone->setPoseRot(currentRot);
+				channelBone->setPoseRot(currentRot);
 				//channelBone->setPoseScale(currentScale);
 			}
 		}
diff --git a/quaternion.h b/quaternion.h
index 358084d..00bb960 100755
--- a/quaternion.h
+++ b/quaternion.h
@@ -36,8 +36,8 @@ namespace vb01{
 			inline Quaternion fromAngle(float angle, Vector3 axis){
 				return Quaternion(cos(angle/2),axis.x*sin(angle/2),axis.y*sin(angle/2),axis.z*sin(angle/2));
 			}
-			inline float getAngle(){return 2*acos(w);}
-			inline Vector3 getAxis(){return getAngle()==0?Vector3(1,0,0):Vector3(x,y,z).norm();}
+			inline float getAngle(){return w > 1 ? 0 : 2*acos(w);}
+			inline Vector3 getAxis(){return x == 0 && y == 0 && z == 0? Vector3(1,0,0) : Vector3(x,y,z).norm();}
 			inline Quaternion operator+(Quaternion q){return Quaternion(w+q.w,x+q.x,y+q.y,z+q.z);}
 			inline Quaternion operator-(Quaternion q){return Quaternion(w-q.w,x-q.x,y-q.y,z-q.z);}
 			template<typename T> inline Quaternion operator*(T s){return Quaternion(w*s,x*s,y*s,z*s);}
diff --git a/skeleton.cpp b/skeleton.cpp
index 00e62d1..f55731c 100644
--- a/skeleton.cpp
+++ b/skeleton.cpp
@@ -29,7 +29,7 @@ namespace vb01{
 		Bone *boneChain[chainLength];
 		Bone *ikBoneAncestor = ikBone;
 		Vector3 boneIkPos[chainLength];
-	   	Vector3 targetPos = ikTarget->getModelSpacePos() + model->globalToLocalPosition(ikTarget->localToGlobalPosition(ikTarget->getPosePos()));
+	   	Vector3 targetPos = ikTarget->getPosition();
 
 		float sumLengths = 0;
 
@@ -42,40 +42,84 @@ namespace vb01{
 
 		Vector3 startPos = boneChain[chainLength - 1]->getModelSpacePos();
 		if(startPos.getDistanceFrom(targetPos) < sumLengths){
-			int numIterations = 500;
+			int numIterations = 2;
 	
 			for(int i = 0 ; i < numIterations; i++){
 				bool backward = (i % 2 == 0);
 				for(int j = 0; j < chainLength; j++){
-					int boneId = (backward ? j : chainLength - 1 - j);
-					Vector3 ikPos = (j == 0 ? (backward ? targetPos : startPos) : boneIkPos[boneId]);
-					Vector3 bonePos = boneChain[boneId]->getModelSpacePos();
-					Vector3 fromBoneToIkPos = (ikPos - bonePos).norm();
-					boneIkPos[boneId] = ikPos - fromBoneToIkPos * boneChain[boneId]->getLength();
+					int boneId;
+					float length;
+					Vector3 ikPos, bonePos, fromBoneToIkPos;
+					if(backward){
+						boneId = j;
+						length = boneChain[boneId]->getLength();
+						ikPos = (j == 0 ? targetPos : boneIkPos[boneId - 1]);
+						bonePos = boneIkPos[boneId];
+						fromBoneToIkPos = (ikPos - bonePos).norm();
+						boneIkPos[boneId] = ikPos - fromBoneToIkPos * length;
+					}
+					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 fromStartToTarget = targetPos - startPos;
-			for(int i = 0; i < chainLength; i++){
+			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;
 			}
 		}
-		*/
 
+		float boneAngles[chainLength];
+		Vector3 axis[chainLength];
 		for(int i = chainLength - 1; i >= 0; i--){
-			Vector3 dir = ((i == 0 ? targetPos : boneIkPos[i - 1]) - boneChain[i]->getModelSpacePos()).norm() * -1;
+			Vector3 dir = ((i == 0 ? targetPos : boneIkPos[i - 1]) - boneIkPos[i]).norm();
 			Vector3 boneAxis = boneChain[i]->getInitAxis(1);
 			Vector3 rotAxis = dir.cross(boneAxis).norm();
-			//rotAxis = (model->localToGlobalPosition(boneChain[i]->globalToLocalPosition(rotAxis))).norm();
 			float angle = boneAxis.getAngleBetween(dir);
+
 			if(rotAxis == Vector3::VEC_ZERO){
 				angle = 0;
 				rotAxis = Vector3::VEC_I;
 			}
 
+			boneAngles[i] = angle; 
+			axis[i] = rotAxis;
+		}
+		for(int i = 0; i < chainLength; i++){
+			for(int j = i + 1; j < chainLength; j++){
+				boneAngles[i] -= boneAngles[j];
+				if(boneAngles[i] <= 0){
+					boneAngles[i] = 0;
+					break;
+				}
+			}
+			float angle = boneAngles[i];
+			Vector3 rotAxis = axis[i];
+
 			boneChain[i]->setPoseRot(Quaternion(angle, rotAxis));
-			//((Node*)boneChain[i])->lookAt(dir, model);
 		}
 	}
 
diff --git a/vbModelReader.cpp b/vbModelReader.cpp
index bf5a193..febd783 100644
--- a/vbModelReader.cpp
+++ b/vbModelReader.cpp
@@ -121,6 +121,10 @@ namespace vb01{
 				yAxis.z = -yAxis.z;
 				swap(zAxis.y, zAxis.z);
 				zAxis.z = -zAxis.z;
+				swap(pos.y, pos.z);
+				pos.z = -pos.z;
+				/*
+				*/
 			}
 			else
 				pos = pos + Vector3(0, ((Bone*)parent)->getLength(), 0);