diff --git a/CMakeLists.txt b/CMakeLists.txt index 98deef5..130deff 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -17,7 +17,7 @@ set(math quaternion.cpp vector.cpp matrix.cpp ray.cpp) set(render particleEmitter.cpp camera.cpp light.cpp material.cpp mesh.cpp model.cpp node.cpp quad.cpp box.cpp root.cpp shader.cpp texture.cpp) set(model modelReader.cpp assimpModelReader.cpp vbModelReader.cpp) set(anim animationController.cpp animationChannel.cpp animation.cpp) -set(armature skeleton.cpp bone.cpp) +set(armature skeleton.cpp bone.cpp ikSolver.cpp) set(library ${render} ${math} ${model} ${utils} ${gui} ${armature} ${anim}) add_library(vb01 SHARED ${library}) @@ -27,7 +27,7 @@ target_link_libraries(vb01 assimp) target_link_libraries(vb01 freetype) set(renderTest nodeTest.cpp cameraTest.cpp shaderTest.cpp particleEmitterTest.cpp) -set(armatureTest boneTest.cpp) +set(armatureTest boneTest.cpp ikSolverTest.cpp) set(modelTest vbModelReaderTest.cpp) set(animationTest animationChannelTest.cpp) set(test ${library} ${renderTest} ${armatureTest} ${modelTest} ${animationTest}) diff --git a/bone.cpp b/bone.cpp index 55e1912..d174001 100644 --- a/bone.cpp +++ b/bone.cpp @@ -53,17 +53,14 @@ namespace vb01{ void Bone::setPoseRot(Quaternion r){ this->poseRot = r; - setOrientation(restRot); + Vector3 rotAxis = r.getAxis(); + rotAxis = + parent->globalToLocalPosition(localToGlobalPosition(rotAxis)) - + parent->globalToLocalPosition(localToGlobalPosition(Vector3::VEC_ZERO)); - Quaternion parentSpacePoseRot = Quaternion::QUAT_W; - vector ancestors = getAncestors(); - for(int i = 0; i < ancestors.size(); i++) - parentSpacePoseRot = parentSpacePoseRot * ancestors[i]->getOrientation(); - for(int i = ancestors.size() - 1; i > 0; i--) - parentSpacePoseRot = parentSpacePoseRot * ancestors[i]->getOrientation().conj(); - parentSpacePoseRot = r * parentSpacePoseRot; + Quaternion parentSpacePoseRot = Quaternion(r.getAngle(), rotAxis); - setOrientation(parentSpacePoseRot); + setOrientation(parentSpacePoseRot * restRot); } void Bone::setPoseScale(Vector3 s){ diff --git a/bone.h b/bone.h index 70cd9c4..e635ad0 100644 --- a/bone.h +++ b/bone.h @@ -20,6 +20,8 @@ namespace vb01{ inline int getIkChainLength(){return ikChainLength;} inline void setIkChainLength(int ikChainLength){this->ikChainLength = ikChainLength;} inline float getLength(){return length;} + inline bool isIkFromTail(){return ikFromTail;} + inline void setIkFromTail(bool ikFromTail){this->ikFromTail = ikFromTail;} inline void setSkeleton(Skeleton *sk){this->skeleton = sk;} inline Vector3 getInitAxis(int i){return initAxis[i];} inline std::string getName(){return name;} @@ -33,6 +35,7 @@ namespace vb01{ float length; Bone *ikTarget = nullptr; int ikChainLength = -1; + bool ikFromTail = true; Skeleton *skeleton = nullptr; Vector3 initAxis[3], restPos, posePos = Vector3::VEC_ZERO, restScale, poseScale = Vector3::VEC_IJK; Quaternion restRot, poseRot = Quaternion::QUAT_W; diff --git a/ikSolver.cpp b/ikSolver.cpp new file mode 100644 index 0000000..c9ef0c4 --- /dev/null +++ b/ikSolver.cpp @@ -0,0 +1,60 @@ +#include "ikSolver.h" +#include "bone.h" + +namespace vb01{ + 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(); + + 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 == 0); + + for(int j = 0; j < chainLength; j++){ + int boneId; + float length; + Vector3 ikPos, bonePos, fromBoneToIkPos; + if(backward){ + boneId = j; + length = boneChain[boneId]->getLength(); + ikPos = (j == 0 ? targetPos : boneIkPos[boneId - 1]); + } + 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 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; + } + } + } +} diff --git a/ikSolver.h b/ikSolver.h new file mode 100644 index 0000000..886045d --- /dev/null +++ b/ikSolver.h @@ -0,0 +1,18 @@ +#ifndef IK_SOLVER_H +#define IK_SOLVER_H + +#include "vector.h" + +namespace vb01{ + class Bone; + + class IkSolver{ + public: + IkSolver(); + ~IkSolver(){} + static void calculateFabrik(int, Bone*[], Vector3[], Vector3); + private: + }; +} + +#endif diff --git a/ikSolverTest.cpp b/ikSolverTest.cpp new file mode 100644 index 0000000..31449c8 --- /dev/null +++ b/ikSolverTest.cpp @@ -0,0 +1,69 @@ +#include "ikSolverTest.h" +#include "ikSolver.h" +#include "skeleton.h" + +namespace vb01{ + void IkSolverTest::setUp(){ + rootNode = Root::getSingleton()->getRootNode(); + + skeleton = new Skeleton(""); + + Bone *boneA = new Bone("A", 1, Vector3::VEC_ZERO, Quaternion::QUAT_W, Vector3::VEC_IJK); + Bone *boneB = new Bone("B", 1, Vector3::VEC_J, Quaternion::QUAT_W, Vector3::VEC_IJK); + Bone *boneC = new Bone("C", 1, Vector3::VEC_J, Quaternion::QUAT_W, Vector3::VEC_IJK); + Bone *boneD = new Bone("D", 1, Vector3::VEC_J, Quaternion::QUAT_W, Vector3::VEC_IJK); + ikBone = new Bone("ik", 1, Vector3(0, 4, 0), Quaternion::QUAT_W, Vector3::VEC_IJK); + + skeleton->addBone(boneA, (Bone*)rootNode); + skeleton->addBone(boneB, boneA); + skeleton->addBone(boneC, boneB); + skeleton->addBone(boneD, boneC); + skeleton->addBone(ikBone, boneA); + + boneA->lookAt(Vector3::VEC_K, Vector3::VEC_J); + boneB->lookAt(Vector3::VEC_K, Vector3::VEC_J); + boneC->lookAt(Vector3::VEC_K, Vector3::VEC_J); + boneD->lookAt(Vector3::VEC_K, Vector3::VEC_J); + + boneD->setIkChainLength(chainLength); + boneD->setIkTarget(ikBone); + + ikPos = new Vector3[chainLength]; + boneChain = new Bone*[chainLength]; + + resetIkData(); + } + + void IkSolverTest::tearDown(){ + } + + void IkSolverTest::testSolveIk(){ + Vector3 ikBonePos = Vector3(1, 1, 1) * 5 + Vector3(0, 1, 0); + resetIkData(); + IkSolver::calculateFabrik(chainLength, boneChain, ikPos, ikBonePos); + + float eps = .01; + + for(int i = chainLength - 1; i >= 0; i--) + CPPUNIT_ASSERT(ikPos[i].getDistanceFrom(Vector3(0, 1, 0) + Vector3(1, 1, 1).norm() * (chainLength - 1 - i)) <= eps); + + ikBonePos = Vector3(0, 1, 0) * 2 + Vector3(2, 0, 0); + 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); + + } + + void IkSolverTest::resetIkData(){ + Bone *rootBone = skeleton->getRootBone(); + Bone *ikBoneAncestor = skeleton->getBone("D"); + + for(int i = 0; i < chainLength; i++){ + boneChain[i] = ikBoneAncestor; + ikBoneAncestor = (Bone*)ikBoneAncestor->getParent(); + ikPos[i] = boneChain[i]->getModelSpacePos(); + } + } +} diff --git a/ikSolverTest.h b/ikSolverTest.h new file mode 100644 index 0000000..1b134d1 --- /dev/null +++ b/ikSolverTest.h @@ -0,0 +1,34 @@ +#ifndef IK_SOLVER_TEST_H +#define IK_SOLVER_TEST_H + +#include +#include +#include "bone.h" + +namespace vb01{ + class Skeleton; + class Node; + class Bone; + + class IkSolverTest : public CppUnit::TestFixture{ + CPPUNIT_TEST_SUITE(IkSolverTest); + CPPUNIT_TEST(testSolveIk); + CPPUNIT_TEST_SUITE_END(); + public: + IkSolverTest(){} + ~IkSolverTest(){} + void setUp(); + void tearDown(); + void testSolveIk(); + private: + Skeleton *skeleton = nullptr; + Node *rootNode = nullptr; + int chainLength = 3; + Vector3 *ikPos; + Bone *ikBone = nullptr, **boneChain; + + void resetIkData(); + }; +} + +#endif diff --git a/main.cpp b/main.cpp index 6f95294..24ad8ad 100755 --- a/main.cpp +++ b/main.cpp @@ -10,6 +10,7 @@ #include "animationChannelTest.h" #include "shaderTest.h" #include "particleEmitterTest.h" +#include "ikSolverTest.h" using namespace CppUnit; using namespace vb01; @@ -23,6 +24,7 @@ int main(){ runner.addTest(AnimationChannelTest::suite()); runner.addTest(ShaderTest::suite()); runner.addTest(ParticleEmitterTest::suite()); + runner.addTest(IkSolverTest::suite()); runner.run(); return 0; } diff --git a/skeleton.cpp b/skeleton.cpp index 0a47bc6..50c75da 100644 --- a/skeleton.cpp +++ b/skeleton.cpp @@ -1,13 +1,14 @@ -#include"skeleton.h" -#include"animation.h" -#include"animationController.h" -#include"box.h" +#include "skeleton.h" +#include "animation.h" +#include "animationController.h" +#include "box.h" +#include "ikSolver.h" using namespace std; namespace vb01{ Skeleton::Skeleton(string name){ - this->name=name; + this->name = name; controller = new AnimationController(this); } @@ -36,66 +37,11 @@ namespace vb01{ boneIkPos[i] = boneChain[i]->getModelSpacePos(); } - calculateFabrik(chainLength, boneChain, boneIkPos, targetPos); + IkSolver::calculateFabrik(chainLength, boneChain, boneIkPos, targetPos); transformIkChain(chainLength, boneChain, boneIkPos, targetPos); } - void Skeleton::calculateFabrik(int chainLength, Bone *boneChain[], Vector3 boneIkPos[], Vector3 targetPos){ - float sumLengths = 0; - for(int i = 0; i < chainLength; i++) - sumLengths += boneChain[i]->getLength(); - - 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 == 0); - for(int j = 0; j < chainLength; j++){ - int boneId; - float length; - Vector3 ikPos, bonePos, fromBoneToIkPos; - if(backward){ - boneId = j; - length = boneChain[boneId]->getLength(); - ikPos = (j == 0 ? targetPos : boneIkPos[boneId - 1]); - } - 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 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; - } - } - } - void Skeleton::transformIkChain(int chainLength, Bone *boneChain[], Vector3 boneIkPos[], Vector3 targetPos){ Node *rootBone = getRootBone()->getParent(); float boneAngles[chainLength]; diff --git a/skeleton.h b/skeleton.h index 550f725..a7270dd 100644 --- a/skeleton.h +++ b/skeleton.h @@ -23,7 +23,6 @@ namespace vb01{ inline int getNumBones(){return bones.size();} private: void solveIk(Bone*); - void calculateFabrik(int, Bone*[], Vector3[], Vector3); void transformIkChain(int, Bone*[], Vector3[], Vector3); std::string name;