Tidy up and get IK working

diff --git a/Makefile b/Makefile
index 6126267b0a18127e708056f75c37cedd21745eb8..91abc4f6fd2de66127fec73718c28f684a1530fd 100644 (file)
--- a/Makefile
+++ b/Makefile
@@ -1,6 +1,6 @@
 all: bin/main
 
-CXX_FLAGS := -g --std=c++17 -Wall -DDEBUG_NODES
+CXX_FLAGS := -g --std=c++17 -march=native -Wall -DDEBUG_NODES -DCOWEDBOY_IK
 
 bin/main: model.o material.o image.o skybox.o program.o ik.o main.o util.o
        clang++ $(CXX_FLAGS) $^ \

diff --git a/ik.cpp b/ik.cpp
index 3895efe7819abdf8a546691f5b436645fd1ac02e..9598ddc870ee0d45b3d8d322ba87aa696d4f3391 100644 (file)
--- a/ik.cpp
+++ b/ik.cpp
@@ -70,7 +70,7 @@ vec3 extractPos(mat4 trans) {
        return vec3(trans[3]);
 }
 
-glm::mat4 absoluteToModelSpace(const Model::Node &root, const Model::Node &n, mat4 m) {
+mat4 absoluteToModelSpace(const Model::Node &root, const Model::Node &n, mat4 m) {
        const Model::Node *parent = &n;
        glm::mat4 res = m;
        std::vector<mat4> trans;
@@ -96,18 +96,24 @@ mat4 getRotationToPoint(vec3 u, vec3 v, float dist) {
        return res;
 }
 
+// Normalizes a transformation matrix to not have any scale factor
+inline mat4 normalizeScale(mat4 m) {
+       for (int i = 0; i < 3; i++)
+               m[i] = normalize(m[i]);
+       return m;
+}
+
 float d = 0;
 
 // Target is world position
-void inverseKinematic(Model::Node &start, Model::Node &end, vec3 target) {
+void inverseKinematics(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;
+       // Calculate the world root
+       const Model::Node &root = start.getRoot();
 
+       // Work out the positions and distances
        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]);
@@ -116,10 +122,9 @@ void inverseKinematic(Model::Node &start, Model::Node &end, vec3 target) {
                        distances[i - 1] = distance(positions[i], positions[i - 1]);
        }
 
+       // Do the actual IK part
        auto newPositions = fabrik(target, positions, distances);
        
-
-       
        // Move 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++) {
@@ -127,26 +132,19 @@ void inverseKinematic(Model::Node &start, Model::Node &end, vec3 target) {
                mat4 absTrans = getAbsTrans(root, node);
                absTrans[3] = vec4(newPositions[i], absTrans[3][3]); // update position in transform
                
-               /* vec3 oldRelPos = extractPos(aiMatrixToMat4(node.ai.mTransformation)); */
-               /* vec3 newRelPos = extractPos(absoluteToModelSpace(root, *node.parent, absTrans)); */
-               /* mat4 rot = getRotationToPoint(oldRelPos, newRelPos, distances[i - 1]); */
-               /* node.ai.mTransformation = mat4ToaiMatrix(rot * aiMatrixToMat4(node.ai.mTransformation)); */
-
-               absTrans[3] = vec4(newPositions[i], absTrans[3][3]); // update position in transform
-               
                mat4 relTrans = absoluteToModelSpace(root, *node.parent, absTrans);
                node.ai.mTransformation = mat4ToaiMatrix(relTrans);
        }
 
        // 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
-       // FIXME: This is numerically unstable and the transformation scales over time!!!
+       // FIXME: Despite normalizeScale, this is still numerically unstable
+       // and the transformation scales over time!!!
        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 nextNodePos = extractPos(aiMatrixToMat4(nextNode.ai.mTransformation));
 
-
                vec3 up = {0, 1, 0};
                vec3 dir = -normalize(nextNodePos);
 
@@ -156,11 +154,14 @@ void inverseKinematic(Model::Node &start, Model::Node &end, vec3 target) {
                                                  -v[1], v[0], 0);
                mat4 rot = mat3(1) + sscpm + sscpm * sscpm * (1.f / 1.f + dot(up, dir));
 
-
+               // very important that we normalize the scale
+               // otherwise we end up with gradually growing models
+               // due to numerical instabaility
+               rot = normalizeScale(rot);
                node.ai.mTransformation = mat4ToaiMatrix(oldTrans * rot);
 
                for (auto child: node.getChildren()) {
-                       child->ai.mTransformation = mat4ToaiMatrix(inverse(rot) * aiMatrixToMat4(child->ai.mTransformation));
+                       child->ai.mTransformation = mat4ToaiMatrix(normalizeScale(inverse(rot)) * aiMatrixToMat4(child->ai.mTransformation));
                }
 
        }

diff --git a/ik.hpp b/ik.hpp
index 2786ffe65cb20d08289661f935e5e492881b3ea5..7c6ebb15124d8303d8c2a9da15a365215094ea5d 100644 (file)
--- a/ik.hpp
+++ b/ik.hpp
@@ -1,4 +1,4 @@
 #include <glm/glm.hpp>
 #include "model.hpp"
 
-void inverseKinematic(Model::Node &end, Model::Node &root, glm::vec3 target);
+void inverseKinematics(Model::Node &end, Model::Node &root, glm::vec3 target);

diff --git a/main.cpp b/main.cpp
index c65e8d249e74f101f31f3ee3b2db817fb609304f..e4cc81a2d79846c9a8e3028cfb85a2b2b42899ab 100644 (file)
--- a/main.cpp
+++ b/main.cpp
@@ -168,41 +168,19 @@ 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) * 2 - 2, 2, 0);
+#ifdef COWEDBOY_IK
+       { 
+               glm::vec3 targetPos(sin(d) * 2 + 3, -2, 1);
                Light targetLight = { glm::translate(glm::mat4(1), targetPos), {0.5, 1, 1}  };
                drawLight(targetLight);
-       inverseKinematic(*sceneModel->find("Bottom Bone"), *sceneModel->find("Toppest Bone"), targetPos);
+               inverseKinematics(*sceneModel->find("Shoulder.L"), *sceneModel->find("Finger.L"), targetPos);
 
-       /* std::array<glm::vec3, 3> jointPositions; std::array<float, 2> jointDistances; */
-
-       /* std::array<std::string, 3> jointNames = { "Bottom Bone", "Middle Bone", "Top Bone" }; */
-       /* for (int i = 0; i < 3; i++) { */
-       /*      glm::mat4 trans; */
-       /*      findNodeTrans(&sceneModel->getRoot()->ai, aiString(jointNames[i]), &trans); */
-       /*      jointPositions[i] = glm::vec3(trans[3]); */
-
-       /*      if (i > 0) */
-       /*              jointDistances[i - 1] = glm::distance(jointPositions[i], jointPositions[i - 1]); */
-       /* } */
-
-       /* glm::vec3 targetPos(sin(d * 10.f), cos(d * 10.f), 0); */
-       /* auto newPositions = fabrik(targetPos, jointPositions, jointDistances); */
-
-       /* 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]); */
-
-       /*      auto node = sceneModel->getRoot()->ai.FindNode(jointNames[i].c_str()); */
-
-       /*      auto newTrans = worldSpaceToModelSpace(node->mParent, newAbsTrans); */
-
-       /*      node->mTransformation = mat4ToaiMatrix(newTrans); */
-       /* } */
-       /* sceneModel->find("Top Bone")->transform = glm::rotate(glm::mat4(1), d / 5.f, { 1, 0, 0}); */
-       /* sceneModel->find("Bottom Bone")->transform = glm::rotate(glm::mat4(1), d / 3.f, { 1, 0, 0}); */
+               targetPos = { sin(d * 2) * 2 - 5, 2.5, 0 };
+               targetLight = { glm::translate(glm::mat4(1), targetPos), {1, 1, 0.5}  };
+               drawLight(targetLight);
+               inverseKinematics(*sceneModel->find("Shoulder.R"), *sceneModel->find("Finger.R"), targetPos);
+       }
+#endif
 
        sceneModel->draw(skyboxes[activeSkybox], d * 1000);
 
@@ -254,7 +232,7 @@ void init() {
        pbrProg = new Program("pbrvert.glsl", "pbrfrag.glsl");
        glUseProgram(pbrProg->progId);
 
-       const std::string scenePath = "models/ik.glb";
+       const std::string scenePath = "models/cowedboy.glb";
        const aiScene *scene = importer.ReadFile(scenePath, aiProcess_Triangulate | aiProcess_CalcTangentSpace | aiProcess_GenNormals | aiProcess_FlipUVs);
        if (!scene) {
                std::cerr << importer.GetErrorString() << std::endl;
@@ -312,7 +290,7 @@ void keyboardUp(unsigned char key, int x, int y) {
        keyStates[key] = false;
 }
 
-#define ENABLE_MOVEMENT
+/* #define ENABLE_MOVEMENT */
 
 void timer(int _) {
 #ifdef ENABLE_MOVEMENT

diff --git a/model.cpp b/model.cpp
index 345309a211e30232e2bd0c50a3945ccf74b43542..f4fc307ebca7db0515ce0af40bc74d916ccc2021 100644 (file)
--- a/model.cpp
+++ b/model.cpp
@@ -224,6 +224,14 @@ glm::mat4 Model::Node::totalTrans(const glm::mat4 parentTrans, const float tick)
        return m;
 }
 
+const Model::Node &Model::Node::getRoot() const {
+       const Model::Node *rootPtr = this;
+       while (rootPtr->parent != nullptr)
+               rootPtr = rootPtr->parent;
+       const Model::Node &root = *rootPtr;
+       return root;
+}
+
 void Model::Node::draw(        const std::vector<Mesh> &meshes,
                                                const std::vector<Material> &materials,
                                                const Skybox skybox,
@@ -341,8 +349,6 @@ Model::Model(const aiScene *scene, Program p): program(p) {
                }
        }
 
-       printHierarchy(scene->mRootNode);
-
        root = new Node(*(scene->mRootNode), p.progId, &animMap, allBones, nullptr);
 }
 

diff --git a/model.hpp b/model.hpp
index e74d65717973400120fd38eb4f2e4a71ba4418b3..a3a290c80096279f31d105265316e23d1d316479 100644 (file)
--- a/model.hpp
+++ b/model.hpp
@@ -73,6 +73,7 @@ class Model {
                                glm::mat4 totalTrans(const glm::mat4 parentTrans, const float tick) const;
 
                                const Node *parent;
+                               const Node &getRoot() const;
 
                                bool operator==(const Node &rhs) const;
 

diff --git a/models/cowedboy.blend b/models/cowedboy.blend
index 5fe3aa3ffeeb3ec073e91486cd1f7dfbee185225..b70977fd65b0f5eeec3d38a6ed0cb6949f783a86 100644 (file)
Binary files a/models/cowedboy.blend and b/models/cowedboy.blend differ

diff --git a/models/cowedboy.glb b/models/cowedboy.glb
new file mode 100644 (file)
index 0000000..ed08600
Binary files /dev/null and b/models/cowedboy.glb differ

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