From 3ee5555652ffe5352cf4027cc0a482648ae15034 Mon Sep 17 00:00:00 2001 From: devZoGok Date: Sat, 29 Aug 2020 16:55:35 +0300 Subject: [PATCH] simplified global - local transformation methods fixed position bug when posing bones initial IK method --- animationController.cpp | 2 +- bone.cpp | 31 ++++++---- bone.h | 15 +++-- node.cpp | 122 ++++++++++++++++++++++++---------------- node.h | 13 +++-- script.py | 26 ++++++++- skeleton.cpp | 51 +++++++++++++++-- skeleton.h | 6 +- vbModelReader.cpp | 16 +++++- 9 files changed, 204 insertions(+), 78 deletions(-) diff --git a/animationController.cpp b/animationController.cpp index 12819ab..eef435c 100644 --- a/animationController.cpp +++ b/animationController.cpp @@ -101,7 +101,7 @@ namespace vb01{ } channelBone->setPosePos(currentPos); - channelBone->setPoseRot(currentRot); + //channelBone->setPoseRot(currentRot); //channelBone->setPoseScale(currentScale); } } diff --git a/bone.cpp b/bone.cpp index b0ec5b3..1235c39 100644 --- a/bone.cpp +++ b/bone.cpp @@ -5,13 +5,22 @@ using namespace std; namespace vb01{ Bone::Bone(string name, float length, Vector3 pos,Quaternion rot, Vector3 scale) : Node(pos,rot,scale,name){ - this->name=name; - this->length=length; + this->name = name; + this->length = length; } void Bone::lookAt(Vector3 newDir, Vector3 newUp, Node *par){ - Node::lookAt(newDir,newUp,par); - for(int i=0;i<3;i++) + Node::lookAt(newDir, newUp, par); + for(int i = 0; i < 3; i++) + this->initAxis[i] = globalAxis[i]; + restPos = pos; + restRot = orientation; + restScale = scale; + } + + void Bone::lookAt(Vector3 newDir, Node *par){ + Node::lookAt(newDir, par); + for(int i = 0; i < 3; i++) this->initAxis[i] = globalAxis[i]; restPos = pos; restRot = orientation; @@ -21,7 +30,7 @@ namespace vb01{ Vector3 Bone::getModelSpacePos(){ Vector3 modelSpacePos = Vector3::VEC_ZERO; Bone *rootBone = skeleton->getRootBone(); - vector boneHierarchy = getAncestors(this, rootBone); + vector boneHierarchy = getAncestors(this, rootBone->getParent()); while(!boneHierarchy.empty()){ int id = boneHierarchy.size() - 1; @@ -43,19 +52,21 @@ namespace vb01{ } void Bone::setPosePos(Vector3 p){ - this->posePos=p; - Vector3 parentSpacePosePos=parent->globalToLocalPosition(localToGlobalPosition(p)); - //setPosition(parentSpacePosePos); + this->posePos = p; + setPosition(restPos); + Vector3 parentSpacePosePos = parent->globalToLocalPosition(localToGlobalPosition(p)); + setPosition(parentSpacePosePos); } void Bone::setPoseRot(Quaternion r){ - this->poseRot=r; + this->poseRot = r; + setOrientation(r); Quaternion parentSpacePoseRot = parent->globalToLocalOrientation(localToGlobalOrientation(r)); setOrientation(parentSpacePoseRot); } void Bone::setPoseScale(Vector3 s){ - this->poseScale=s; + this->poseScale = s; //setScale(restScale+poseScale); } } diff --git a/bone.h b/bone.h index 46a9183..3d2d178 100644 --- a/bone.h +++ b/bone.h @@ -9,12 +9,17 @@ namespace vb01{ class Bone : public Node{ public: - Bone(std::string,float,Vector3=Vector3::VEC_ZERO,Quaternion=Quaternion::QUAT_W,Vector3=Vector3::VEC_ZERO); + Bone(std::string, float, Vector3 = Vector3::VEC_ZERO, Quaternion = Quaternion::QUAT_W, Vector3 = Vector3::VEC_ZERO); void setPosePos(Vector3 p); void setPoseRot(Quaternion r); void setPoseScale(Vector3 s); - void lookAt(Vector3,Vector3,Node*); + void lookAt(Vector3, Vector3, Node*); + void lookAt(Vector3, Node*); Vector3 getModelSpacePos(); + inline Bone* getIkTarget(){return ikTarget;} + inline void setIkTarget(Bone *target){this->ikTarget = target;} + inline int getIkChainLength(){return ikChainLength;} + inline void setIkChainLength(int ikChainLength){this->ikChainLength = ikChainLength;} inline float getLength(){return length;} inline void setSkeleton(Skeleton *sk){this->skeleton = sk;} inline Vector3 getInitAxis(int i){return initAxis[i];} @@ -27,9 +32,11 @@ namespace vb01{ inline Vector3 getPoseScale(){return poseScale;} private: float length; + Bone *ikTarget = nullptr; + int ikChainLength = -1; Skeleton *skeleton = nullptr; - Vector3 initAxis[3],restPos,posePos=Vector3::VEC_ZERO,restScale,poseScale=Vector3::VEC_IJK; - Quaternion restRot,poseRot=Quaternion::QUAT_W; + Vector3 initAxis[3], restPos, posePos = Vector3::VEC_ZERO, restScale, poseScale = Vector3::VEC_IJK; + Quaternion restRot, poseRot = Quaternion::QUAT_W; }; } diff --git a/node.cpp b/node.cpp index c4fe4e1..278803d 100755 --- a/node.cpp +++ b/node.cpp @@ -101,14 +101,22 @@ namespace vb01{ text->setNode(this); } - void Node::lookAt(Vector3 newDir,Vector3 newUp,Node *node){ + void Node::lookAt(Vector3 newDir, Vector3 newUp, Node *node){ + adjustDir(newDir, node); + adjustUp(newUp, node); + } + + void Node::lookAt(Vector3 newDir, Node *node){ + adjustDir(newDir, node); + } + + void Node::adjustDir(Vector3 newDir, Node *node){ Vector3 parAxis[]{ node->getGlobalAxis(0), node->getGlobalAxis(1), node->getGlobalAxis(2) }; newDir=(parAxis[0]*newDir.x+parAxis[1]*newDir.y+parAxis[2]*newDir.z).norm(); - newUp=(parAxis[0]*newUp.x+parAxis[1]*newUp.y+parAxis[2]*newUp.z).norm(); float angle=globalAxis[2].getAngleBetween(newDir); Vector3 rotAxis=globalAxis[2].cross(newDir).norm(); @@ -118,6 +126,15 @@ namespace vb01{ Quaternion oldRot = node->localToGlobalOrientation(orientation); Quaternion newRot = node->globalToLocalOrientation(Quaternion(angle,rotAxis)); setOrientation(newRot*oldRot); + } + + void Node::adjustUp(Vector3 newUp, Node *node){ + Vector3 parAxis[]{ + node->getGlobalAxis(0), + node->getGlobalAxis(1), + node->getGlobalAxis(2) + }; + newUp=(parAxis[0]*newUp.x+parAxis[1]*newUp.y+parAxis[2]*newUp.z).norm(); mat3 mat; mat[0][0]=globalAxis[0].x; @@ -133,10 +150,10 @@ namespace vb01{ vec3 nu=vec3(newUp.x,newUp.y,newUp.z)*mat; newUp=(globalAxis[0]*nu.x+globalAxis[1]*nu.y).norm(); - angle=globalAxis[1].getAngleBetween(newUp); + float angle=globalAxis[1].getAngleBetween(newUp); - oldRot = node->localToGlobalOrientation(orientation); - newRot = node->globalToLocalOrientation(Quaternion(angle*(nu.x<0?1:-1),globalAxis[2])); + Quaternion oldRot = node->localToGlobalOrientation(orientation); + Quaternion newRot = node->globalToLocalOrientation(Quaternion(angle*(nu.x<0?1:-1),globalAxis[2])); setOrientation(newRot*oldRot); } @@ -151,7 +168,7 @@ namespace vb01{ vector Node::getAncestors(Node *node, Node *topAncestor){ vector ancestors; - Node *parent=node; + Node *parent = node; if(!topAncestor) topAncestor = Root::getSingleton()->getRootNode(); @@ -159,7 +176,7 @@ namespace vb01{ ancestors.push_back(parent); if(parent == topAncestor) break; - parent=parent->getParent(); + parent = parent->getParent(); } return ancestors; } @@ -171,34 +188,25 @@ namespace vb01{ parent->getGlobalAxis(2) }; - float rotAngle=orientation.getAngle(); - Vector3 rotAxis=orientation.getAxis(); - Vector3 newAxis=(parGlobalAxis[0]*rotAxis.x+parGlobalAxis[1]*rotAxis.y+parGlobalAxis[2]*rotAxis.z).norm(); + float rotAngle = orientation.getAngle(); + Vector3 rotAxis = orientation.getAxis(); + Vector3 newAxis = (parGlobalAxis[0] * rotAxis.x + parGlobalAxis[1] * rotAxis.y + parGlobalAxis[2] * rotAxis.z).norm(); Quaternion rotQuat=Quaternion(rotAngle,newAxis); - for(int i=0;i<3;i++) + for(int i = 0; i < 3; i++) globalAxis[i] = (rotQuat * parGlobalAxis[i]).norm(); for(Node *ch : children) ch->updateAxis(); } Vector3 Node::localToGlobalPosition(Vector3 localPos){ - vector ancestors=getAncestors(this); - Vector3 origin=Vector3::VEC_ZERO; + vector ancestors = getAncestors(this); + Vector3 origin = Vector3::VEC_ZERO; + Quaternion quat; - while(!ancestors.empty()){ - int id=ancestors.size()-1; - Node *par=ancestors[id]->getParent(); - Vector3 parAxis[]={ - par?par->getGlobalAxis(0):Vector3::VEC_I, - par?par->getGlobalAxis(1):Vector3::VEC_J, - par?par->getGlobalAxis(2):Vector3::VEC_K - }; - Vector3 p=ancestors[id]->getPosition(); - origin=origin+parAxis[0]*p.x+parAxis[1]*p.y+parAxis[2]*p.z; - ancestors.pop_back(); - } - return origin+globalAxis[0]*localPos.x+globalAxis[1]*localPos.y+globalAxis[2]*localPos.z; + adjustPosOrRot(ancestors, origin, quat, true); + + return origin + globalAxis[0] * localPos.x + globalAxis[1] * localPos.y + globalAxis[2] * localPos.z; } Vector3 Node::globalToLocalPosition(Vector3 globalPos){ @@ -219,43 +227,59 @@ namespace vb01{ } Quaternion Node::localToGlobalOrientation(Quaternion localRot){ - Quaternion origin=Quaternion::QUAT_W; - vector ancestors=getAncestors(this); + Vector3 vec; + Quaternion origin = Quaternion::QUAT_W; + vector ancestors = getAncestors(this); - while(!ancestors.empty()){ - int id=ancestors.size()-1; - Vector3 ancAxis[]{ - ancestors[id]->getGlobalAxis(0), - ancestors[id]->getGlobalAxis(1), - ancestors[id]->getGlobalAxis(2) - }; - float angle = ancestors[id]->getOrientation().getAngle(); - Vector3 axis = ancestors[id]->getOrientation().getAxis(); - Quaternion o = Quaternion(angle, (ancAxis[0]*axis.x+ancAxis[1]*axis.y+ancAxis[2]*axis.z).norm()); - origin=o*origin; - ancestors.pop_back(); - } - return localRot*origin; + adjustPosOrRot(ancestors, vec, origin, true); + + return localRot * origin; } Quaternion Node::globalToLocalOrientation(Quaternion globalRot){ - Quaternion origin=globalRot; - vector ancestors=getAncestors(this); + Vector3 vec; + Quaternion origin = globalRot; + vector ancestors = getAncestors(this); + + adjustPosOrRot(ancestors, vec, origin, false); + + return origin; + } + + void Node::adjustPosOrRot(vector ancestors, Vector3 &adjustablePos, Quaternion &adjustableRot, bool localToGlobal){ + Vector3 pOrigin = Vector3::VEC_ZERO; + Quaternion rOrigin = Quaternion::QUAT_W; while(!ancestors.empty()){ - int id=ancestors.size()-1; - Vector3 ancAxis[]{ + int id = ancestors.size() - 1; + Vector3 parAxis[]{ ancestors[id]->getGlobalAxis(0), ancestors[id]->getGlobalAxis(1), ancestors[id]->getGlobalAxis(2) }; + + Vector3 p = ancestors[id]->getPosition(); + pOrigin = pOrigin + parAxis[0] * p.x + parAxis[1] * p.y + parAxis[2] * p.z; + float angle = ancestors[id]->getOrientation().getAngle(); Vector3 axis = ancestors[id]->getOrientation().getAxis(); - Quaternion o = Quaternion(-angle, (ancAxis[0]*axis.x+ancAxis[1]*axis.y+ancAxis[2]*axis.z).norm()); - origin=origin*o; + Vector3 rotAxis = (parAxis[0] * axis.x + parAxis[1] * axis.y + parAxis[2] * axis.z).norm(); + + Quaternion o; + if(localToGlobal){ + o = Quaternion(angle, rotAxis); + rOrigin = rOrigin * o; + } + else{ + o = Quaternion(-angle, rotAxis); + rOrigin = o * rOrigin; + } + ancestors.pop_back(); } - return origin; + + adjustablePos = pOrigin; + adjustableRot = rOrigin; } void Node::updateShaders(){ diff --git a/node.h b/node.h index f5a346e..d0581dd 100755 --- a/node.h +++ b/node.h @@ -16,11 +16,11 @@ namespace vb01{ class Node{ public: struct Transform{ - Vector3 position=Vector3::VEC_ZERO,scale=Vector3::VEC_IJK; - Quaternion orientation=Quaternion::QUAT_W; + Vector3 position = Vector3::VEC_ZERO, scale = Vector3::VEC_IJK; + Quaternion orientation = Quaternion::QUAT_W; }; - Node(Vector3=Vector3::VEC_ZERO,Quaternion=Quaternion::QUAT_W,Vector3=Vector3::VEC_IJK,std::string=""); + Node(Vector3 = Vector3::VEC_ZERO, Quaternion = Quaternion::QUAT_W, Vector3 = Vector3::VEC_IJK, std::string = ""); virtual ~Node(); void attachMesh(Mesh*); void attachParticleEmitter(ParticleEmitter*); @@ -30,7 +30,8 @@ namespace vb01{ void addLight(Light*); void removeLight(int); void addText(Text*); - virtual void lookAt(Vector3,Vector3,Node*); + virtual void lookAt(Vector3, Vector3, Node*); + virtual void lookAt(Vector3, Node*); void updateAxis(); void setOrientation(Quaternion); void getDescendants(Node*,std::vector&); @@ -61,6 +62,10 @@ namespace vb01{ inline bool isVisible(){return visible;} inline std::string getName(){return name;} inline void setVisible(bool v){this->visible=v;} + private: + void adjustUp(Vector3, Node*); + void adjustDir(Vector3, Node*); + void adjustPosOrRot(std::vector, Vector3&, Quaternion&, bool); protected: void updateShaders(); diff --git a/script.py b/script.py index be8ae35..be55f81 100644 --- a/script.py +++ b/script.py @@ -20,7 +20,25 @@ def exportData(ob): tail=bone.tail xAxis=bone.x_axis yAxis=bone.y_axis - file.write("\n"+bone.name+": "+('None' if bone.parent is None else bone.parent.name)+" "+str(bone.length)+" "+str(head.x)+" "+str(head.y)+" "+str(head.z)+" "+str(xAxis.x)+" "+str(xAxis.y)+" "+str(xAxis.z)+" "+str(yAxis.x)+" "+str(yAxis.y)+" "+str(yAxis.z)+" ") + ikTarget = '-' + chainLength = -1 + + ikConstraint = None + for constr in ob.pose.bones[bone.name].constraints: + if constr.name == 'IK': + ikConstraint = constr + if(ikConstraint is not None and ikConstraint.subtarget): + ikTarget = ikConstraint.subtarget + if ikConstraint.chain_count == 0: + chainLength = 1 + bonePar = bone.parent + while(bonePar is not None): + chainLength = chain_count + 1 + bonePar = bonePar.parent + else: + chainLength = ikConstraint.chain_count + + file.write('\n'+bone.name+': '+('None' if bone.parent is None else bone.parent.name)+" "+str(bone.length)+" "+str(head.x)+" "+str(head.y)+" "+str(head.z)+" "+str(xAxis.x)+" "+str(xAxis.y)+" "+str(xAxis.z)+" "+str(yAxis.x)+" "+str(yAxis.y)+" "+str(yAxis.z)+ " " + ikTarget + ' ' + str(chainLength) + ' ') if numAnims>0: file.write("\nanimations: "+str(numAnims)) @@ -85,7 +103,11 @@ def exportData(ob): file.write(str(keyframe.handle_right_type)+" "+str(keyframe.handle_right.x)+" "+str(keyframe.handle_right.y)) file.write("\n") elif(ob.type=="MESH"): - file.write('skeleton: ' + ob.modifiers['Armature'].object.name + '\n') + skeletonName = '-' + skeleton = ob.modifiers['Armature'].object + if(skeleton): + skeletonName = skeleton.name + file.write('skeleton: ' + skeletonName + '\n') mesh=ob.data numFaces=len(mesh.polygons) diff --git a/skeleton.cpp b/skeleton.cpp index b9ff627..bcd0ae1 100644 --- a/skeleton.cpp +++ b/skeleton.cpp @@ -12,13 +12,56 @@ namespace vb01{ } void Skeleton::update(){ - /* - for(Bone *b : bones) - b->update(); - */ + for(Bone *b : bones){ + //b->update(); + if(b->getIkTarget()) + solveIk(b); + } controller->update(); } + void Skeleton::solveIk(Bone *ikBone){ + const int chainLength = ikBone->getIkChainLength(); + Bone *ikTarget = ikBone->getIkTarget(); + Bone *boneChain[chainLength]; + Bone *ikBoneAncestor = ikBone; + Vector3 boneIkPos[chainLength], targetPos = ikTarget->getModelSpacePos(); + float sumLengths = 0; + + for(int i = 0; i < chainLength; i++){ + sumLengths += ikBoneAncestor->getLength(); + boneChain[i] = ikBoneAncestor; + ikBoneAncestor = (Bone*)ikBoneAncestor->getParent(); + boneIkPos[i] = boneChain[i]->getModelSpacePos(); + } + + 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; + for(int j = 0; j < chainLength; j++){ + int boneId = (backward ? j : chainLength - 1 - j); + Vector3 ikPos = (j == 0 ? (backward ? targetPos : startPos) : boneIkPos[boneId]); + Vector3 fromBoneToIkPos = ikPos - boneChain[boneId]->getModelSpacePos(); + boneIkPos[boneId] = ikPos - fromBoneToIkPos * boneChain[boneId]->getLength(); + } + } + } + /* + else{ + Vector3 fromStartToTarget = targetPos - startPos; + for(int i = 0; i < chainLength; i++){ + } + } + */ + + Bone *rootBone = getRootBone(); + for(int i = 0; i < chainLength; i++) + (boneChain[i])->lookAt(boneIkPos[i], rootBone->getParent()); + } + void Skeleton::addBone(Bone *bone, Bone *parent){ if(parent) parent->attachChild(bone); diff --git a/skeleton.h b/skeleton.h index 9986ae2..1115555 100644 --- a/skeleton.h +++ b/skeleton.h @@ -11,9 +11,9 @@ namespace vb01{ class Skeleton{ public: - Skeleton(std::string=""); + Skeleton(std::string = ""); void update(); - void addBone(Bone*,Bone*); + void addBone(Bone*, Bone*); Bone* getBone(std::string); inline AnimationController* getAnimationController(){return controller;} inline Bone* getBone(int i){return bones[i];} @@ -22,6 +22,8 @@ namespace vb01{ inline std::string getName(){return name;} inline int getNumBones(){return bones.size();} private: + void solveIk(Bone*); + std::string name; std::vector bones; AnimationController *controller = nullptr; diff --git a/vbModelReader.cpp b/vbModelReader.cpp index 558e61b..bf5a193 100644 --- a/vbModelReader.cpp +++ b/vbModelReader.cpp @@ -92,12 +92,13 @@ namespace vb01{ meshData.clear(); readFile(path, meshData, boneStartLine, boneStartLine + numBones); + vector ikRelationships; for(string line : meshData){ int colonId = getCharId(line, ':'); string preColon = line.substr(0, colonId); - string postColon = line.substr(colonId + 2, string::npos); + string postColon = line.substr(colonId + 2); - int numData = 11; + int numData = 13; string data[numData]; getLineData(postColon, data, numData); @@ -105,6 +106,8 @@ namespace vb01{ Vector3 head = Vector3(atof(data[2].c_str()), atof(data[3].c_str()), atof(data[4].c_str())); Vector3 xAxis = Vector3(atof(data[5].c_str()), atof(data[6].c_str()), atof(data[7].c_str())); Vector3 yAxis = Vector3(atof(data[8].c_str()), atof(data[9].c_str()), atof(data[10].c_str())); + string ikTarget = data[11].c_str(); + int ikChainLength = atoi(data[12].c_str()); Vector3 zAxis = xAxis.cross(yAxis); Vector3 pos = head; @@ -123,8 +126,17 @@ namespace vb01{ pos = pos + Vector3(0, ((Bone*)parent)->getLength(), 0); Bone *bone = new Bone(preColon, length, pos); + bone->setIkChainLength(ikChainLength); skeleton->addBone(bone, (Bone*)parent); bone->lookAt(zAxis, yAxis, parent); + if(ikTarget != "-") + ikRelationships.push_back(preColon + " " + ikTarget); + } + for(string ikRelationship : ikRelationships){ + int spaceId = getCharId(ikRelationship, ' '); + string targetBoneName = ikRelationship.substr(0, spaceId); + string ikTargetBoneName = ikRelationship.substr(spaceId + 1); + skeleton->getBone(targetBoneName)->setIkTarget(skeleton->getBone(ikTargetBoneName)); } AnimationController *controller = skeleton->getAnimationController();