attached objects follow their bones

model space positions are retrieved from bones
supported more than 4 bones
improved global and local orientation methods
improved vector angle method

bone.cpp

@@ -1,30 +1,61 @@
 #include"bone.h"
+#include"skeleton.h"
 
 using namespace std;
 
 namespace vb01{
-	Bone::Bone(string name, Vector3 axis[3], Vector3 pos,Quaternion rot, Vector3 scale) : Node(pos,rot,scale){
+	Bone::Bone(string name, float length, Vector3 pos,Quaternion rot, Vector3 scale) : Node(pos,rot,scale,name){
 		this->name=name;
-		this->restPos=pos;
-		this->restRot=rot;
-		this->restScale=scale;
+		this->length=length;
+	}
+
+	void Bone::lookAt(Vector3 newDir, Vector3 newUp, Node *par){
+		Node::lookAt(newDir,newUp,par);
 		for(int i=0;i<3;i++)
-			this->initAxis[i]=axis[i];
-		//lookAt(axis[1],axis[2],parent);
+			this->initAxis[i] = globalAxis[i];
+		restPos = pos;
+		restRot = orientation;
+		restScale = scale;
+	}
+
+	Vector3 Bone::getModelSpacePos(){
+		Vector3 modelSpacePos = Vector3::VEC_ZERO;
+		Bone *rootBone = skeleton->getRootBone();
+		vector<Node*> boneHierarchy = getAncestors(this, rootBone);
+
+		while(!boneHierarchy.empty()){
+			int id = boneHierarchy.size() - 1;
+			Bone *curBone = ((Bone*)boneHierarchy[id]);
+			Bone *par = (Bone*)curBone->getParent();
+
+			Vector3 axis[]{ 
+				curBone != rootBone? par->getInitAxis(0) : Vector3::VEC_I,
+				curBone != rootBone? par->getInitAxis(1) : Vector3::VEC_J,
+				curBone != rootBone? par->getInitAxis(2) : Vector3::VEC_K
+			};
+
+			Vector3 rPos = curBone->getRestPos();
+			modelSpacePos = modelSpacePos + axis[0] * rPos.x + axis[1] * rPos.y + axis[2] * rPos.z;
+			boneHierarchy.pop_back();
+		}
+
+		return modelSpacePos;
 	}
 
 	void Bone::setPosePos(Vector3 p){
 		this->posePos=p;
-		setPosition(restPos+posePos);
+		Vector3 parentSpacePosePos=parent->globalToLocalPosition(localToGlobalPosition(p));
+		//setPosition(parentSpacePosePos);
 	}
 
 	void Bone::setPoseRot(Quaternion r){
 		this->poseRot=r;
-		setOrientation(poseRot*restRot);
+		Quaternion parentSpacePoseRot = parent->globalToLocalOrientation(localToGlobalOrientation(r));
+		setOrientation(parentSpacePoseRot);
 	}
 
 	void Bone::setPoseScale(Vector3 s){
 		this->poseScale=s;
-		setScale(restScale+poseScale);
+		//setScale(restScale+poseScale);
 	}
 }