diff --git a/bone.cpp b/bone.cpp index 87c1842..4f9dfbd 100644 --- a/bone.cpp +++ b/bone.cpp @@ -11,6 +11,10 @@ namespace vb01{ void Bone::lookAt(Vector3 newDir, Vector3 newUp){ Node::lookAt(newDir, newUp); + updateBoneInfo(); + } + + void Bone::updateBoneInfo(){ for(int i = 0; i < 3; i++) this->initAxis[i] = globalAxis[i]; restPos = pos; @@ -18,11 +22,10 @@ namespace vb01{ restScale = scale; } - Vector3 Bone::getModelSpacePos(){ - /* - Vector3 modelSpacePos = Vector3::VEC_ZERO; + Vector3 Bone::getBoneSpaceRestPos(Bone *bone){ Bone *rootBone = skeleton->getRootBone(); - vector boneHierarchy = getAncestors(rootBone); + Vector3 modelSpacePos = Vector3::VEC_ZERO; + vector boneHierarchy = getAncestors(bone); while(!boneHierarchy.empty()){ int id = boneHierarchy.size() - 1; @@ -35,11 +38,15 @@ namespace vb01{ curBone != rootBone? par->getInitAxis(2) : Vector3::VEC_K }; - Vector3 rPos = curBone->getRestPos(); + Vector3 rPos = (curBone == bone ? Vector3::VEC_ZERO : curBone->getRestPos()); modelSpacePos = modelSpacePos + axis[0] * rPos.x + axis[1] * rPos.y + axis[2] * rPos.z; boneHierarchy.pop_back(); } - */ + + return modelSpacePos; + } + + Vector3 Bone::getModelSpacePos(){ Node *modelNode = skeleton->getRootBone()->getParent(); Vector3 modelSpacePos = modelNode->globalToLocalPosition(localToGlobalPosition(Vector3::VEC_ZERO)); diff --git a/bone.h b/bone.h index e635ad0..eb1845e 100644 --- a/bone.h +++ b/bone.h @@ -14,6 +14,7 @@ namespace vb01{ void setPoseRot(Quaternion r); void setPoseScale(Vector3 s); void lookAt(Vector3, Vector3); + Vector3 getBoneSpaceRestPos(Bone*); Vector3 getModelSpacePos(); inline Bone* getIkTarget(){return ikTarget;} inline void setIkTarget(Bone *target){this->ikTarget = target;} @@ -32,6 +33,8 @@ namespace vb01{ inline Quaternion getPoseRot(){return poseRot;} inline Vector3 getPoseScale(){return poseScale;} private: + void updateBoneInfo(); + float length; Bone *ikTarget = nullptr; int ikChainLength = -1; diff --git a/boneTest.cpp b/boneTest.cpp index ca90b04..1512396 100644 --- a/boneTest.cpp +++ b/boneTest.cpp @@ -41,25 +41,26 @@ namespace vb01{ float eps = .0001; Bone *lowerArm = skeleton->getBone("lowerArm.R"); Vector3 testModelSpacePos = Vector3(-1, 4, 0); + Bone *rootBone = skeleton->getRootBone(); - CPPUNIT_ASSERT(lowerArm->getModelSpacePos().getDistanceFrom(testModelSpacePos) <= eps); + CPPUNIT_ASSERT(lowerArm->getBoneSpaceRestPos(rootBone).getDistanceFrom(testModelSpacePos) <= eps); modelNode->setPosition(Vector3::VEC_ZERO); modelNode->setOrientation(Quaternion(.5, Vector3(1, 2, 3).norm())); - CPPUNIT_ASSERT(lowerArm->getModelSpacePos().getDistanceFrom(testModelSpacePos) <= eps); + CPPUNIT_ASSERT(lowerArm->getBoneSpaceRestPos(rootBone).getDistanceFrom(testModelSpacePos) <= eps); modelNode->setOrientation(Quaternion::QUAT_W); modelNode->setPosition(Vector3(10, 20, 30)); - CPPUNIT_ASSERT(lowerArm->getModelSpacePos().getDistanceFrom(testModelSpacePos) <= eps); + CPPUNIT_ASSERT(lowerArm->getBoneSpaceRestPos(rootBone).getDistanceFrom(testModelSpacePos) <= eps); modelNode->setOrientation(Quaternion(.5, Vector3(1, 2, 3).norm())); modelNode->setPosition(Vector3(10, 20, 30)); - CPPUNIT_ASSERT(lowerArm->getModelSpacePos().getDistanceFrom(testModelSpacePos) <= eps); + CPPUNIT_ASSERT(lowerArm->getBoneSpaceRestPos(rootBone).getDistanceFrom(testModelSpacePos) <= eps); modelNodeParent->setOrientation(Quaternion(.3, Vector3(1, 2, 3).norm())); modelNodeParent->setPosition(Vector3(100, 200, 300)); modelNode->setOrientation(Quaternion(.5, Vector3(1, 2, 3).norm())); modelNode->setPosition(Vector3(10, 20, 30)); - CPPUNIT_ASSERT(lowerArm->getModelSpacePos().getDistanceFrom(testModelSpacePos) <= eps); + CPPUNIT_ASSERT(lowerArm->getBoneSpaceRestPos(rootBone).getDistanceFrom(testModelSpacePos) <= eps); } } diff --git a/ikSolver.cpp b/ikSolver.cpp index c9ef0c4..5701a81 100644 --- a/ikSolver.cpp +++ b/ikSolver.cpp @@ -7,7 +7,8 @@ namespace vb01{ for(int i = 0; i < chainLength; i++) sumLengths += boneChain[i]->getLength(); - Vector3 startPos = boneChain[chainLength - 1]->getModelSpacePos(); + Bone *subBase = (Bone*)boneChain[0]->getIkTarget()->getParent(); + Vector3 startPos = boneChain[chainLength - 1]->getBoneSpaceRestPos(subBase); if(startPos.getDistanceFrom(targetPos) < sumLengths){ int numIterations = 500; diff --git a/ikSolverTest.cpp b/ikSolverTest.cpp index dbc26f0..8bec0c7 100644 --- a/ikSolverTest.cpp +++ b/ikSolverTest.cpp @@ -51,9 +51,9 @@ namespace vb01{ 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); + /* */ } diff --git a/skeleton.cpp b/skeleton.cpp index 50c75da..993d299 100644 --- a/skeleton.cpp +++ b/skeleton.cpp @@ -3,8 +3,11 @@ #include "animationController.h" #include "box.h" #include "ikSolver.h" +#include +#include using namespace std; +using namespace glm; namespace vb01{ Skeleton::Skeleton(string name){ @@ -22,51 +25,83 @@ namespace vb01{ } void Skeleton::solveIk(Bone *ikBone){ - const int chainLength = ikBone->getIkChainLength(); - Bone *rootBone = getRootBone(); Bone *ikTarget = ikBone->getIkTarget(); + Bone *subBase = (Bone*)ikTarget->getParent(); + + const int chainLength = ikBone->getIkChainLength(); + Vector3 boneIkPos[chainLength]; Bone *boneChain[chainLength]; Bone *ikBoneAncestor = ikBone; - Vector3 boneIkPos[chainLength]; - Vector3 targetPos = rootBone->globalToLocalPosition(ikTarget->localToGlobalPosition(Vector3::VEC_ZERO)); for(int i = 0; i < chainLength; i++){ boneChain[i] = ikBoneAncestor; ikBoneAncestor = (Bone*)ikBoneAncestor->getParent(); - boneIkPos[i] = rootBone->globalToLocalPosition(boneChain[i]->localToGlobalPosition(Vector3::VEC_ZERO)); - boneIkPos[i] = boneChain[i]->getModelSpacePos(); + boneIkPos[i] = boneChain[i]->getBoneSpaceRestPos(subBase); } + Vector3 targetPos = subBase->globalToLocalPosition(ikTarget->localToGlobalPosition(Vector3::VEC_ZERO)); IkSolver::calculateFabrik(chainLength, boneChain, boneIkPos, targetPos); - - transformIkChain(chainLength, boneChain, boneIkPos, targetPos); + transformIkChain(chainLength, boneChain, boneIkPos, ikTarget); } - void Skeleton::transformIkChain(int chainLength, Bone *boneChain[], Vector3 boneIkPos[], Vector3 targetPos){ - Node *rootBone = getRootBone()->getParent(); - float boneAngles[chainLength]; - Vector3 axis[chainLength]; - for(int i = chainLength - 1; i >= 0; i--){ - boneChain[i]->setOrientation(boneChain[i]->getRestRot()); - Vector3 dir = ((i == 0 ? targetPos : boneIkPos[i - 1]) - boneIkPos[i]).norm(); - Vector3 boneAxis = - (rootBone->globalToLocalPosition(boneChain[i]->localToGlobalPosition(Vector3::VEC_J)) - - rootBone->globalToLocalPosition(boneChain[i]->localToGlobalPosition(Vector3::VEC_ZERO))) - .norm(); + void Skeleton::transformIkChain(int chainLength, Bone *boneChain[], Vector3 boneIkPos[], Bone* ikTarget){ + Bone *subBase = (Bone*)ikTarget->getParent(); + Vector3 rotAxis[chainLength]; + float angles[chainLength]; - Vector3 rotAxis = boneAxis.cross(dir).norm(); - float angle = boneAxis.getAngleBetween(dir); - - if(rotAxis == Vector3::VEC_ZERO){ - angle = 0; - rotAxis = Vector3::VEC_I; + for(int i = 0; i < chainLength; i++){ + Vector3 tailPosBoneSpace; + Vector3 tailRestPosBoneSpace; + if(i == 0){ + tailPosBoneSpace = subBase->globalToLocalPosition(ikTarget->localToGlobalPosition(Vector3::VEC_ZERO)); + tailRestPosBoneSpace = ikTarget->getBoneSpaceRestPos(subBase); } + else{ + tailPosBoneSpace = boneIkPos[i - 1]; + tailRestPosBoneSpace = boneChain[i - 1]->getBoneSpaceRestPos(subBase); + } + Vector3 headRestPosBoneSpace = boneChain[i]->getBoneSpaceRestPos(subBase); + Vector3 headDir = (tailPosBoneSpace - headRestPosBoneSpace).norm(); + Vector3 boneDir = (tailRestPosBoneSpace - headRestPosBoneSpace).norm(); - boneAngles[i] = angle; - axis[i] = rotAxis; + float angle = boneDir.getAngleBetween(headDir); + angles[i] = angle; - boneChain[i]->setPoseRot(Quaternion(angle, rotAxis)); + Vector3 axis = boneDir.cross(headDir).norm(); + if(axis == Vector3::VEC_ZERO) + axis = Vector3::VEC_I; + + axis = ( + subBase->getInitAxis(0) * axis.x + + subBase->getInitAxis(1) * axis.y + + subBase->getInitAxis(2) * axis.z + ).norm(); + + mat3 mat; + mat[0][0] = boneChain[i]->getInitAxis(0).x; + mat[1][0] = boneChain[i]->getInitAxis(0).y; + mat[2][0] = boneChain[i]->getInitAxis(0).z; + mat[0][1] = boneChain[i]->getInitAxis(1).x; + mat[1][1] = boneChain[i]->getInitAxis(1).y; + mat[2][1] = boneChain[i]->getInitAxis(1).z; + mat[0][2] = boneChain[i]->getInitAxis(2).x; + mat[1][2] = boneChain[i]->getInitAxis(2).y; + mat[2][2] = boneChain[i]->getInitAxis(2).z; + vec3 localAxis = vec3(axis.x, axis.y, axis.z) * inverse(mat); + + rotAxis[i] = Vector3(localAxis.x, localAxis.y, localAxis.z).norm(); } + + for(int i = 0; i < chainLength; i++){ + for(int j = i + 1; j < chainLength; j++) + angles[i] -= angles[j]; + + if(angles[i] < 0) + angles[i] = 0; + } + + for(int i = 0; i < chainLength; i++) + boneChain[i]->setPoseRot(Quaternion(angles[i], rotAxis[i])); } void Skeleton::addBone(Bone *bone, Bone *parent){ diff --git a/skeleton.h b/skeleton.h index a7270dd..a7ec2f8 100644 --- a/skeleton.h +++ b/skeleton.h @@ -23,7 +23,7 @@ namespace vb01{ inline int getNumBones(){return bones.size();} private: void solveIk(Bone*); - void transformIkChain(int, Bone*[], Vector3[], Vector3); + void transformIkChain(int, Bone*[], Vector3[], Bone*); std::string name; std::vector bones;