From: Luke Lau Date: Sun, 16 Feb 2020 21:09:24 +0000 (+0000) Subject: IK is working!!! X-Git-Tag: cs7gv5-a2~2 X-Git-Url: https://git.lukelau.me/?p=opengl.git;a=commitdiff_plain;h=17dcc398b9a5d1a6a287c01dff7fc7cf2b792372 IK is working!!! --- diff --git a/ik.cpp b/ik.cpp index fe4ecbb..c6fa603 100644 --- 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 fabrik(const glm::vec3 t, const std::vector 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 chain = allNodesTo(root, end); +// Target is world position +void inverseKinematic(Model::Node &start, Model::Node &end, vec3 target) { + std::vector 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 positions(chain.size()); std::vector 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 a579adf..c65e8d2 100644 --- 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 jointPositions; std::array jointDistances; */ diff --git a/models/ik.blend b/models/ik.blend index 5442399..37c97a9 100644 Binary files a/models/ik.blend and b/models/ik.blend differ diff --git a/models/ik.glb b/models/ik.glb index 8203a05..5c1deba 100644 Binary files a/models/ik.glb and b/models/ik.glb differ