From: Luke Lau Date: Sun, 16 Feb 2020 01:40:32 +0000 (+0000) Subject: WIP on inverse kinematics X-Git-Tag: cs7gv5-a2~3 X-Git-Url: https://git.lukelau.me/?p=opengl.git;a=commitdiff_plain;h=d26c67a51e58c3f70d1689e265e9bebe578b50ad WIP on inverse kinematics --- diff --git a/Makefile b/Makefile index 5d9c00c..6126267 100644 --- a/Makefile +++ b/Makefile @@ -1,8 +1,8 @@ all: bin/main -CXX_FLAGS := -g --std=c++17 -Wall +CXX_FLAGS := -g --std=c++17 -Wall -DDEBUG_NODES -bin/main: model.o material.o image.o skybox.o program.o main.o util.o +bin/main: model.o material.o image.o skybox.o program.o ik.o main.o util.o clang++ $(CXX_FLAGS) $^ \ -I/usr/local/include -L/usr/local/lib \ -lassimp \ diff --git a/compile_flags.txt b/compile_flags.txt index 5617deb..7ecd5cf 100644 --- a/compile_flags.txt +++ b/compile_flags.txt @@ -1,2 +1,4 @@ +-Wall -std=c++17 -I/usr/local/include/ +-DDEBUG_NODES diff --git a/ik.cpp b/ik.cpp new file mode 100644 index 0000000..fe4ecbb --- /dev/null +++ b/ik.cpp @@ -0,0 +1,159 @@ +#include "ik.hpp" +#include "util.hpp" + +using namespace glm; + +constexpr float tolerance = 0.3; + +const std::vector fabrik(const glm::vec3 t, + const std::vector jpsIn, // joint positions + const std::vector jds // distances between each joint + ) { + size_t N = jpsIn.size(); + assert(N == jds.size() + 1); + std::vector jps = jpsIn; + + float dist = distance(jps[0], t); + float totalLength = 0; + for (int i = 0; i < N - 1; i++) totalLength += jds[i]; + if (dist > totalLength) { // target is unreachable + for (int i = 0; i < N - 1; i++) { + float r = distance(t, jps[i]); + float lambda = jds[i] / r; + jps[i + 1] = (1.f - lambda) * jps[i] + lambda * t; + } + } else { // target is reachable + glm::vec3 b = jps[0]; + // distance between end effector and target + float diff = distance(jps[N - 1], t); + while (diff > tolerance) { + // forward reaching + jps[N - 1] = t; // set end effector to target + for (int i = N - 2; i >= 0; i--) { + float r = distance(jps[i + 1], jps[i]); + float lambda = jds[i] / r; + jps[i] = (1.f - lambda) * jps[i + 1] + lambda * jps[i]; + } + + // backward reaching + jps[0] = b; // reset root to initial pos + for (int i = 0; i < N - 1; i++) { + float r = distance(jps[i + 1], jps[i]); + float lambda = jds[i] / r; + jps[i + 1] = (1 - lambda) * jps[i] + lambda * jps[i + 1]; + } + + float newDiff = distance(jps[N - 1], t); + if (newDiff == diff) abort(); + diff = newDiff; + } + } + return jps; +} + +std::vector allNodesTo(const Model::Node &from, const Model::Node &to) { + if (from == to) return { to }; + assert(to.parent != nullptr); + auto res = allNodesTo(from, *to.parent); + res.push_back(to); + return res; +} + +mat4 getAbsTrans(const Model::Node &root, const Model::Node &n) { + if (root.ai.mName == n.ai.mName) + return mat4(1); //aiMatrixToMat4(n.ai.mTransformation); + assert(n.parent != nullptr); + return getAbsTrans(root, *n.parent) * aiMatrixToMat4(n.ai.mTransformation); +} + +vec3 extractPos(mat4 trans) { + return vec3(trans[3]); +} + +glm::mat4 absoluteToModelSpace(const Model::Node &root, const Model::Node &n, mat4 m) { + const Model::Node *parent = &n; + glm::mat4 res = m; + std::vector trans; + while (&parent->ai != &root.ai) { + trans.push_back(inverse(aiMatrixToMat4(parent->ai.mTransformation))); + parent = parent->parent; + } + while (!trans.empty()) { res = trans.back() * res; trans.pop_back(); } + return res; +} + +// Given points u and v on a sphere, calculate the transformation matrix +// to bring u -> v +// from https://math.stackexchange.com/a/2765250 +mat4 getRotationToPoint(vec3 u, vec3 v, float dist) { + if (distance(u, v) < 0.00001) return mat4(1); + vec3 n = cross(u, v) / length(cross(u, v)); + vec3 t = cross(n, u); + float alpha = atan2(dot(v, t), dot(v, u)); + mat4 T = mat4(mat3(u, t, n)); + mat4 R = rotate(mat4(1), alpha, {0, 0, 1}); // rotation in z axis + mat4 res = T * R * inverse(T); + return res; +} + + +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 chain = allNodesTo(root, end); + assert(!chain.empty()); + + std::vector positions(chain.size()); std::vector distances(chain.size() - 1); + for (size_t i = 0; i < chain.size(); i++) { + mat4 absTrans = getAbsTrans(root, chain[i]); + positions[i] = extractPos(absTrans); + if (i > 0) + 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 + for (size_t i = 1; i < chain.size(); i++) { + auto node = chain[i]; + 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)); + + /* 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); + + /* absTrans[3] = vec4(newPositions[i], absTrans[3][3]); // update position in transform */ + + /* mat4 relTrans = absoluteToModelSpace(root, *node.parent, absTrans); */ + /* node.ai.mTransformation = mat4ToaiMatrix(relTrans); */ + } + + // TODO: Now rotate all the nodes so that they face each other + + /* 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); */ + /* } */ +} + diff --git a/ik.hpp b/ik.hpp new file mode 100644 index 0000000..2786ffe --- /dev/null +++ b/ik.hpp @@ -0,0 +1,4 @@ +#include +#include "model.hpp" + +void inverseKinematic(Model::Node &end, Model::Node &root, glm::vec3 target); diff --git a/main.cpp b/main.cpp index 281da27..a579adf 100644 --- a/main.cpp +++ b/main.cpp @@ -21,6 +21,7 @@ #include "skybox.hpp" #include "image.hpp" #include "util.hpp" +#include "ik.hpp" #pragma clang diagnostic ignored "-Wdeprecated-declarations" @@ -101,6 +102,33 @@ void drawLight(Light &light) { glDrawArrays(GL_TRIANGLES, 0, 36); } +int findNodeTrans(const struct aiNode *n, const struct aiString name, glm::mat4 *dest) { + if (strcmp(n->mName.data, name.data) == 0) { + *dest = aiMatrixToMat4(n->mTransformation); + return 0; + } + for (int i = 0; i < n->mNumChildren; i++) { + if (findNodeTrans(n->mChildren[i], name, dest) == 0) { + glm::mat4 t = aiMatrixToMat4(n->mTransformation); + *dest = t * *dest; + return 0; + } + } + return 1; +} + +glm::mat4 worldSpaceToModelSpace(aiNode *node, glm::mat4 m) { + aiNode *parent = node; + glm::mat4 res = m; + std::vector trans; + while (parent != nullptr) { + /* res = res * glm::inverse(aiMatrixToMat4(parent->mTransformation)); */ + trans.push_back(glm::inverse(aiMatrixToMat4(parent->mTransformation))); + parent = parent->mParent; + } + while (!trans.empty()) { res = trans.back() * res; trans.pop_back(); } + return res; +} void display() { glClearColor(0.5, 0.5, 0.5, 1); @@ -140,8 +168,40 @@ 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)); + inverseKinematic(*sceneModel->find("Bottom Bone"), *sceneModel->find("Toppest Bone"), targetPos); + + /* std::array jointPositions; std::array jointDistances; */ + + /* std::array 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}); */ + sceneModel->draw(skyboxes[activeSkybox], d * 1000); for (Light &light: lights) drawLight(light); @@ -175,20 +235,6 @@ void setupLightBuffers(GLuint progId) { glVertexAttribPointer(posLoc, 3, GL_FLOAT, GL_FALSE, 0, 0); } -int findNodeTrans(struct aiNode *n, const struct aiString name, glm::mat4 *dest) { - if (strcmp(n->mName.data, name.data) == 0) { - *dest = aiMatrixToMat4(n->mTransformation); - return 0; - } - for (int i = 0; i < n->mNumChildren; i++) { - if (findNodeTrans(n->mChildren[i], name, dest) == 0) { - glm::mat4 t = aiMatrixToMat4(n->mTransformation); - *dest = t * *dest; - return 0; - } - } - return 1; -} void init() { initUtilProg(); @@ -206,7 +252,7 @@ void init() { pbrProg = new Program("pbrvert.glsl", "pbrfrag.glsl"); glUseProgram(pbrProg->progId); - const std::string scenePath = "models/newtonsCradle.glb"; + const std::string scenePath = "models/ik.glb"; const aiScene *scene = importer.ReadFile(scenePath, aiProcess_Triangulate | aiProcess_CalcTangentSpace | aiProcess_GenNormals | aiProcess_FlipUVs); if (!scene) { std::cerr << importer.GetErrorString() << std::endl; @@ -269,6 +315,9 @@ void keyboardUp(unsigned char key, int x, int y) { void timer(int _) { #ifdef ENABLE_MOVEMENT float xSpeed = 0.f, ySpeed = 0.f, zSpeed = 0.f; + +#pragma clang diagnostic push +#pragma clang diagnostic ignored "-Wchar-subscripts" if (keyStates['w']) zSpeed = 0.1f; if (keyStates['s']) @@ -281,6 +330,7 @@ void timer(int _) { ySpeed = 0.1f; if (keyStates['e']) ySpeed = -0.1f; +#pragma clang diagnostic pop camPos.x += xSpeed * sin(yaw) + zSpeed * cos(yaw); camPos.y += ySpeed; diff --git a/model.cpp b/model.cpp index 5302df0..345309a 100644 --- a/model.cpp +++ b/model.cpp @@ -115,13 +115,13 @@ Model::Mesh::Mesh(const aiMesh *aiMesh, GLuint progId) { glVertexAttribPointer(boneWeightLoc, 4, GL_FLOAT, GL_FALSE, sizeof(VertBones), (const GLvoid *)sizeof(VertBones::ids)); } -Model::Node::Node(const aiNode &node, GLuint progId, AnimMap *am): ai(node), progId(progId), animMap(am) { +Model::Node::Node(aiNode &node, GLuint progId, AnimMap *am, std::set allBones, Node *p): ai(node), parent(p), progId(progId), animMap(am), isBone(allBones.count(std::string(node.mName.C_Str())) > 0) { for (int i = 0; i < node.mNumMeshes; i++) { meshIndices.push_back(node.mMeshes[i]); } for (int i = 0; i < node.mNumChildren; i++) { - const aiNode *child = node.mChildren[i]; - children.push_back(new Node(*child, progId, am)); + aiNode *child = node.mChildren[i]; + children.push_back(new Node(*child, progId, am, allBones, this)); } } @@ -235,6 +235,9 @@ void Model::Node::draw( const std::vector &meshes, glm::mat4 m = totalTrans(parentTrans, tick); #ifdef DEBUG_NODES + if (isBone) + drawDebugNode(m, {0, 0.5, 1, 1}); + else drawDebugNode(m); #endif @@ -291,13 +294,6 @@ void Model::Node::draw( const std::vector &meshes, for (Node *child: children) child->draw(meshes, materials, skybox, tick, boneTransforms, m); } -void printMatrix4x4(aiMatrix4x4 m) { - fprintf(stderr, "%f, %f, %f, %f\n", m.a1, m.a2, m.a3, m.a4); - fprintf(stderr, "%f, %f, %f, %f\n", m.b1, m.b2, m.b3, m.b4); - fprintf(stderr, "%f, %f, %f, %f\n", m.c1, m.c2, m.c3, m.c4); - fprintf(stderr, "%f, %f, %f, %f\n", m.d1, m.d2, m.d3, m.d4); -} - void printHierarchy(aiNode *n, int indent = 0) { for (int i = 0; i < indent; i++) fprintf(stderr, "\t"); @@ -310,9 +306,12 @@ void printHierarchy(aiNode *n, int indent = 0) { Model::Model(const aiScene *scene, Program p): program(p) { glUseProgram(p.progId); + std::set allBones; for (int i = 0; i < scene->mNumMeshes; i++) { const aiMesh *mesh = scene->mMeshes[i]; meshes.push_back(Mesh(mesh, p.progId)); + for (int j = 0; j < mesh->mNumBones; j++) + allBones.insert(std::string(mesh->mBones[j]->mName.C_Str())); } for (unsigned int i = 0; i < scene->mNumMaterials; i++) { @@ -342,7 +341,9 @@ Model::Model(const aiScene *scene, Program p): program(p) { } } - root = new Node(*(scene->mRootNode), p.progId, &animMap); + printHierarchy(scene->mRootNode); + + root = new Node(*(scene->mRootNode), p.progId, &animMap, allBones, nullptr); } @@ -393,3 +394,7 @@ Model::Node* Model::Node::findNode(const aiNode &aiNode) { } return nullptr; } + +bool Model::Node::operator==(const Model::Node &rhs) const { + return &ai == &rhs.ai; +} diff --git a/model.hpp b/model.hpp index bf6f08f..e74d657 100644 --- a/model.hpp +++ b/model.hpp @@ -1,3 +1,6 @@ +#ifndef MODEL_HPP +#define MODEL_HPP + #include #include #include @@ -20,6 +23,14 @@ inline glm::mat4 aiMatrixToMat4(aiMatrix4x4 from) { return to; } +inline aiMatrix4x4 mat4ToaiMatrix(glm::mat4 from) { + aiMatrix4x4 to; + for (int i = 0; i < 4; i++) + for (int j = 0; j < 4; j++) + to[i][j] = from[j][i]; + return to; +} + class Model { struct Animation { @@ -49,23 +60,29 @@ class Model { class Node { public: - Node(const aiNode &aiNode, GLuint progId, AnimMap *animMap); + Node(aiNode &aiNode, GLuint progId, AnimMap *animMap, std::set allBones, Node *parent); + void draw(const std::vector &meshes, const std::vector &materials, const Skybox s, const float tick, const BoneTransforms &boneTransforms, glm::mat4 parentModel) const; const std::vector &getChildren() const { return children; } Node* findNode(const aiNode &aiNode); - const aiNode &ai; + aiNode &ai; // an extra transform glm::mat4 transform = glm::mat4(1); glm::mat4 totalTrans(const glm::mat4 parentTrans, const float tick) const; + const Node *parent; + + bool operator==(const Node &rhs) const; + private: const GLuint progId; const AnimMap *animMap; std::vector children; std::vector meshIndices; + const bool isBone; }; Node* getRoot() { return root; } @@ -85,3 +102,5 @@ class Model { BoneTransforms calcBoneTransforms(const Node &n, const float tick, const std::set bones, const glm::mat4 parentTrans) const; void loadModel(const std::string &path); }; + +#endif diff --git a/models/ik.blend b/models/ik.blend index 247fb50..5442399 100644 Binary files a/models/ik.blend and b/models/ik.blend differ diff --git a/models/ik.glb b/models/ik.glb index 33539b7..8203a05 100644 Binary files a/models/ik.glb and b/models/ik.glb differ diff --git a/util.cpp b/util.cpp index 796017d..c7b3f4d 100644 --- a/util.cpp +++ b/util.cpp @@ -45,3 +45,14 @@ void drawDebugNode(glm::mat4 model, glm::vec4 color) { glUseProgram(prevProg); } + +void printMatrix4x4(aiMatrix4x4 m) { + fprintf(stderr, "%f, %f, %f, %f\n", m.a1, m.a2, m.a3, m.a4); + fprintf(stderr, "%f, %f, %f, %f\n", m.b1, m.b2, m.b3, m.b4); + fprintf(stderr, "%f, %f, %f, %f\n", m.c1, m.c2, m.c3, m.c4); + fprintf(stderr, "%f, %f, %f, %f\n", m.d1, m.d2, m.d3, m.d4); +} + +void printVec3(glm::vec3 v) { + fprintf(stderr, "{ %f, %f, %f }\n", v[0], v[1], v[2]); +} diff --git a/util.hpp b/util.hpp index 89236e2..84b2060 100644 --- a/util.hpp +++ b/util.hpp @@ -1,6 +1,9 @@ #include "program.hpp" #include +#include void initUtilProg(); Program *getUtilProg(); void drawDebugNode(glm::mat4 transform, glm::vec4 color = {1, 0.5, 1, 1}); +void printMatrix4x4(aiMatrix4x4 m); +void printVec3(glm::vec3 v);