IK is working!!!

diff --git a/ik.cpp b/ik.cpp
index fe4ecbb06eea8355f736c0a279b64764cea7870a..c6fa6030748bbfb4740b5099d872651d57451874 100644 (file)
--- a/ik.cpp
+++ b/ik.cpp
@@ -3,7 +3,7 @@
 
 using namespace glm;
 
-constexpr float tolerance = 0.3;
+constexpr float tolerance = 0.001;
 
 const std::vector<glm::vec3> fabrik(const glm::vec3 t,
                const std::vector<glm::vec3> jpsIn, // joint positions
@@ -96,16 +96,18 @@ mat4 getRotationToPoint(vec3 u, vec3 v, float dist) {
        return res;
 }
 
+float d = 0;
 
-void inverseKinematic(Model::Node &root, Model::Node &end, vec3 target) {
-       /* float s2o2 = sqrt(2.f) / 2.f; */
-       /* assert(getRotationToPoint({1, 0, 0}, {0, s2o2, s2o2}, 1) */
-       /*              == mat4({0, s2o2, s2o2, 0},  { -s2o2, 1.f/2.f, -1.f/2.f, 0}, */
-       /*                              {-s2o2, -1.f/2.f, 1.f/2.f, 0}, { 0, 0, 0, 1})); */
-
-       std::vector<Model::Node> chain = allNodesTo(root, end);
+// Target is world position
+void inverseKinematic(Model::Node &start, Model::Node &end, vec3 target) {
+       std::vector<Model::Node> chain = allNodesTo(start, end);
        assert(!chain.empty());
 
+       const Model::Node *rootPtr = start.parent;
+       while (rootPtr->parent != nullptr)
+               rootPtr = rootPtr->parent;
+       const Model::Node &root = *rootPtr;
+
        std::vector<vec3> positions(chain.size()); std::vector<float> distances(chain.size() - 1);
        for (size_t i = 0; i < chain.size(); i++) {
                mat4 absTrans = getAbsTrans(root, chain[i]);
@@ -114,10 +116,10 @@ void inverseKinematic(Model::Node &root, Model::Node &end, vec3 target) {
                        distances[i - 1] = distance(positions[i], positions[i - 1]);
        }
 
-       /* glm::vec3 targetPos(sin(d * 10.f), cos(d * 10.f), 0); */
        auto newPositions = fabrik(target, positions, distances);
 
        // Rotate all the nodes so that they are in the correct positions
+       // Don't need to move the root node - it's already in place
        for (size_t i = 1; i < chain.size(); i++) {
                auto node = chain[i];
                mat4 absTrans = getAbsTrans(root, node);
@@ -132,7 +134,7 @@ void inverseKinematic(Model::Node &root, Model::Node &end, vec3 target) {
                /* std::cerr << node.ai.mName.C_Str() << ":\n"; */
                /* printVec3(extractPos(aiMatrixToMat4(node.ai.mTransformation))); */
                /* printVec3(newRelPos); */
-               assert(distance(extractPos(aiMatrixToMat4(node.ai.mTransformation)), newRelPos) < 0.0001);
+               /* assert(distance(extractPos(aiMatrixToMat4(node.ai.mTransformation)), newRelPos) < 0.0001); */
 
                /* absTrans[3] = vec4(newPositions[i], absTrans[3][3]); // update position in transform */
                
@@ -140,20 +142,60 @@ void inverseKinematic(Model::Node &root, Model::Node &end, vec3 target) {
                /* node.ai.mTransformation = mat4ToaiMatrix(relTrans); */
        }
 
-       // TODO: Now rotate all the nodes so that they face each other
+       // TODO: Now rotate all the nodes so that they point towards each other
+       // Don't need to rotate the last node - it has nothing to point towards
+       for (size_t i = 0; i < chain.size() - 1; i++) {
+               auto node = chain[i]; auto nextNode = chain[i + 1];
+               mat4 oldTrans = aiMatrixToMat4(node.ai.mTransformation);
+               vec3 nodePos = extractPos(oldTrans);
+               vec3 nextNodePos = extractPos(aiMatrixToMat4(nextNode.ai.mTransformation));
+
+
+               /* vec3 up = normalize(oldTrans[1]); */
+               vec3 up = {0, 1, 0};
+               vec3 dir = -normalize(nextNodePos);
+               /* mat4 rot = mat3(aiMatrixToMat4(nextNode.ai.mTransformation)); */
+               /* mat4 rot = transpose(lookAt(vec3(0), dir, {0, 1, 0})); */
+
+               vec3 v = cross(up, dir);
+               mat3 sscpm = mat3(0, -v[2], v[1],
+                                                 v[2], 0, -v[0],
+                                                 -v[1], v[0], 0);
+               mat4 rot = mat3(1) + sscpm + sscpm * sscpm * (1.f / 1.f + dot(up, dir));
+               /* mat4 rot = rotate(mat4(1), 0.01f, {0, 1, 0}); */
+               /* mat4 rot = mat4(1); */
+
+
+               mat4 trans = oldTrans * rot * inverse(oldTrans);
+               node.ai.mTransformation = mat4ToaiMatrix(trans * oldTrans);
+               for (auto child: node.getChildren()) {
+                       child->ai.mTransformation = mat4ToaiMatrix(inverse(rot) * aiMatrixToMat4(child->ai.mTransformation));
+               }
+
                
-       /* for (int i = 0; i < 3; i++) { */
-       /*      glm::mat4 absTrans(1); */
-       /*      findNodeTrans(&sceneModel->getRoot()->ai, aiString(jointNames[i]), */
-       /*                      &absTrans); */
-       /*      glm::mat4 newAbsTrans = absTrans; */
-       /*      newAbsTrans[3] = glm::vec4(newPositions[i], newAbsTrans[3][3]); */
+               /* vec3 nextNodePos = extractPos(oldTrans * aiMatrixToMat4(nextNode.ai.mTransformation)); */
+               /* vec3 d = normalize(nextNodePos - nodePos); */
 
-       /*      auto node = sceneModel->getRoot()->ai.FindNode(jointNames[i].c_str()); */
+               /* mat4 m = mat3(d.z, 0, -d.x, 0, 1, 0, d.x, 0, d.z); */
 
-       /*      auto newTrans = worldSpaceToModelSpace(node->mParent, newAbsTrans); */
+               /* vec3 up = oldTrans[1]; */
+               /* mat4 look = lookAt(vec3(0), nextNodePos, up); */
+               /* look = oldTrans * look * inverse(oldTrans); */
+               /* node.ai.mTransformation = mat4ToaiMatrix(m * oldTrans); */
 
-       /*      node->mTransformation = mat4ToaiMatrix(newTrans); */
+               /* for (auto child: node.getChildren()) { */
+               /*      child->ai.mTransformation = mat4ToaiMatrix(inverse(m) * aiMatrixToMat4(child->ai.mTransformation)); */
                /* } */
+
+               /* vec3 pos = extractPos(oldTrans); */
+               /* vec3 up = normalize(translate(oldTrans, -pos)); */
+               /* vec3 up = oldTrans[1]; */
+               /* mat4 rot = lookAt(vec3(0), extractPos(aiMatrixToMat4(nextNode.ai.mTransformation)), up); */
+               /* node.ai.mTransformation = mat4ToaiMatrix(translate(rot * translate(oldTrans, -pos), pos)); */
+               /* for (auto child: node.getChildren()) { */
+               /*      child->ai.mTransformation = mat4ToaiMatrix(inverse(rot) * aiMatrixToMat4(child->ai.mTransformation)); */
+               /* } */
+       }
+
 }
 

diff --git a/main.cpp b/main.cpp
index a579adfc63ffdb71e910f63840b1a4b625bd9711..c65e8d249e74f101f31f3ee3b2db817fb609304f 100644 (file)
--- a/main.cpp
+++ b/main.cpp
@@ -168,7 +168,9 @@ void display() {
        glUniform3fv(glGetUniformLocation(pbrProg->progId, "lightPositions"), numLights, glm::value_ptr(lightPositions[0]));
        glUniform3fv(glGetUniformLocation(pbrProg->progId, "lightColors"), numLights, glm::value_ptr(lightColors[0]));
 
-       glm::vec3 targetPos(sin(d * 1.f), 0, cos(d * 1.f));
+       glm::vec3 targetPos(sin(d) * 2 - 2, 2, 0);
+       Light targetLight = { glm::translate(glm::mat4(1), targetPos), {0.5, 1, 1}  };
+       drawLight(targetLight);
        inverseKinematic(*sceneModel->find("Bottom Bone"), *sceneModel->find("Toppest Bone"), targetPos);
 
        /* std::array<glm::vec3, 3> jointPositions; std::array<float, 2> jointDistances; */

diff --git a/models/ik.blend b/models/ik.blend
index 544239981adc6d3ae1efe08c9166543a048f2c83..37c97a90673873b5fd921913e6767cede94ec707 100644 (file)
Binary files a/models/ik.blend and b/models/ik.blend differ

diff --git a/models/ik.glb b/models/ik.glb
index 8203a059d030f1310d6245f79fc3738d8504d241..5c1deba4fb2ae8863c5c6639fa17ac0b6ab9f183 100644 (file)
Binary files a/models/ik.glb and b/models/ik.glb differ