diff --git a/bone.cpp b/bone.cpp index d47e39a..1434731 100644 --- a/bone.cpp +++ b/bone.cpp @@ -17,11 +17,17 @@ namespace vb01{ void Bone::updateBoneInfo(){ for(int i = 0; i < 3; i++) this->initAxis[i] = globalAxis[i]; + restPos = pos; restRot = orientation; restScale = scale; } + void Bone::onAttached(){ + Node::onAttached(); + updateBoneInfo(); + } + Vector3 Bone::getBoneSpaceRestPos(Bone *bone){ Bone *rootBone = skeleton->getRootBone(); Vector3 modelSpacePos = Vector3::VEC_ZERO; diff --git a/bone.h b/bone.h index 92b65fa..815422d 100644 --- a/bone.h +++ b/bone.h @@ -36,6 +36,7 @@ namespace vb01{ private: void updateBoneInfo(); void animate(float, KeyframeChannel); + void onAttached(); virtual float getDriverValue(Driver::VariableType); float length; diff --git a/ikSolver.cpp b/ikSolver.cpp index 041d041..58df42e 100644 --- a/ikSolver.cpp +++ b/ikSolver.cpp @@ -5,12 +5,14 @@ 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(); Skeleton *skeleton = boneChain[0]->getSkeleton(); Bone *subBase = (Bone*)skeleton->getBone(boneChain[0]->getIkTarget())->getParent(); Vector3 startPos = subBase->globalToLocalPosition(boneChain[chainLength - 1]->localToGlobalPosition(Vector3::VEC_ZERO)); + if(startPos.getDistanceFrom(targetPos) < sumLengths){ int numIterations = 500; @@ -21,6 +23,7 @@ namespace vb01{ int boneId; float length; Vector3 ikPos, bonePos, fromBoneToIkPos; + if(backward){ boneId = j; length = boneChain[boneId]->getLength(); @@ -28,6 +31,7 @@ namespace vb01{ } else{ boneId = chainLength - 1 - j; + if(j == 0){ ikPos = startPos; length = 0; @@ -37,6 +41,7 @@ namespace vb01{ length = boneChain[boneId + 0]->getLength(); } } + bonePos = boneIkPos[boneId]; fromBoneToIkPos = (ikPos - bonePos).norm(); boneIkPos[boneId] = ikPos - fromBoneToIkPos * length; @@ -45,9 +50,11 @@ namespace vb01{ } 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]; @@ -56,6 +63,7 @@ namespace vb01{ length = boneChain[i]->getLength(); bonePos = boneIkPos[i + 1]; } + boneIkPos[i] = bonePos + startToEndVec * length; } } diff --git a/node.cpp b/node.cpp index b55ea71..6fa076a 100755 --- a/node.cpp +++ b/node.cpp @@ -111,6 +111,7 @@ namespace vb01{ child->setParent(this); children.push_back(child); child->updateAxis(); + child->onAttached(); } void Node::dettachChild(Node *child){ @@ -184,11 +185,12 @@ namespace vb01{ Vector3 xDir = orientation * Vector3(1, 0, 0), yDir = orientation * Vector3(0, 1, 0), zDir = orientation * Vector3(0, 0, 1); - newUp = Vector3(newUp.x, newUp.y, 0).norm(); + //newUp = Vector3(newUp.x, newUp.y, 0).norm(); if(newUp != Vector3::VEC_ZERO){ float angle = yDir.getAngleBetween(newUp); - setOrientation(Quaternion(angle * (newUp.x < 0 ? 1 : -1), zDir) * orientation); + bool forw = (xDir.getAngleBetween(newUp) < PI / 2); + setOrientation(Quaternion(angle * (forw ? -1 : 1), zDir) * orientation); } } diff --git a/node.h b/node.h index 4d4e370..1b88f76 100755 --- a/node.h +++ b/node.h @@ -27,6 +27,7 @@ namespace vb01{ void attachMesh(Mesh*); void attachParticleEmitter(ParticleEmitter*); virtual void update(); + virtual void onAttached(){} void attachChild(Node*); void dettachChild(Node*); void addLight(Light*); diff --git a/skeleton.cpp b/skeleton.cpp index 74568ad..47e4a38 100644 --- a/skeleton.cpp +++ b/skeleton.cpp @@ -24,14 +24,17 @@ namespace vb01{ if(b->getIkTarget() != ""){ const int chainLength = b->getIkChainLength(); Bone **boneChain = getIkBoneChain(b); + for(int i = 0; i < chainLength; i++){ boneChain[i]->setPosePos(Vector3::VEC_ZERO); boneChain[i]->setPoseRot(Quaternion::QUAT_W); boneChain[i]->setPoseScale(Vector3::VEC_IJK); } + delete[] boneChain; } } + for(Bone *b : bones) if(getBone(b->getIkTarget())) solveIk(b); @@ -44,6 +47,7 @@ namespace vb01{ Bone **boneChain = getIkBoneChain(ikBone); Vector3 *boneIkPos = new Vector3[chainLength]; + for(int i = chainLength - 1; i >= 0; i--) boneIkPos[i] = subBase->globalToLocalPosition(boneChain[i]->localToGlobalPosition(Vector3::VEC_ZERO)); @@ -57,10 +61,11 @@ namespace vb01{ void Skeleton::transformIkChain(int chainLength, Bone **boneChain, Vector3 boneIkPos[], Bone* ikTarget){ Bone *subBase = (Bone*)ikTarget->getParent(); + for(int i = chainLength - 1; i >= 0; i--){ boneChain[i]->setPoseRot(Quaternion::QUAT_W); - Vector3 tailPosBoneSpace; + if(i == 0) tailPosBoneSpace = subBase->globalToLocalPosition(ikTarget->localToGlobalPosition(Vector3::VEC_ZERO)); else @@ -75,6 +80,7 @@ namespace vb01{ axis = boneChain[i]->globalToLocalPosition(subBase->localToGlobalPosition(axis)) - boneChain[i]->globalToLocalPosition(subBase->localToGlobalPosition(Vector3::VEC_ZERO)); + if(axis == Vector3::VEC_ZERO) axis = Vector3::VEC_I; @@ -86,17 +92,20 @@ namespace vb01{ void Skeleton::addBone(Bone *bone, Bone *parent){ if(parent) parent->attachChild(bone); + bone->setSkeleton(this); bones.push_back(bone); } Bone* Skeleton::getBone(string name){ Bone *bone = nullptr; + for(Bone *b : bones) if(b->getName() == name){ bone = b; break; } + return bone; } @@ -104,10 +113,12 @@ namespace vb01{ int chainLength = ikBone->getIkChainLength(); Bone **chain = new Bone*[chainLength]; Bone *ikBoneAncestor = ikBone; + for(int i = 0; i < chainLength; i++){ chain[i] = ikBoneAncestor; ikBoneAncestor = (Bone*)ikBoneAncestor->getParent(); } + return chain; } }