X-Git-Url: https://git.lukelau.me/?p=opengl.git;a=blobdiff_plain;f=ik.cpp;fp=ik.cpp;h=fe4ecbb06eea8355f736c0a279b64764cea7870a;hp=0000000000000000000000000000000000000000;hb=d26c67a51e58c3f70d1689e265e9bebe578b50ad;hpb=5e2b8438a1600a790dc105d25b7d5cd2278864aa 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); */ + /* } */ +} +