6 constexpr float tolerance = 0.3;
8 const std::vector<glm::vec3> fabrik(const glm::vec3 t,
9 const std::vector<glm::vec3> jpsIn, // joint positions
10 const std::vector<float> jds // distances between each joint
12 size_t N = jpsIn.size();
13 assert(N == jds.size() + 1);
14 std::vector<glm::vec3> jps = jpsIn;
16 float dist = distance(jps[0], t);
17 float totalLength = 0;
18 for (int i = 0; i < N - 1; i++) totalLength += jds[i];
19 if (dist > totalLength) { // target is unreachable
20 for (int i = 0; i < N - 1; i++) {
21 float r = distance(t, jps[i]);
22 float lambda = jds[i] / r;
23 jps[i + 1] = (1.f - lambda) * jps[i] + lambda * t;
25 } else { // target is reachable
27 // distance between end effector and target
28 float diff = distance(jps[N - 1], t);
29 while (diff > tolerance) {
31 jps[N - 1] = t; // set end effector to target
32 for (int i = N - 2; i >= 0; i--) {
33 float r = distance(jps[i + 1], jps[i]);
34 float lambda = jds[i] / r;
35 jps[i] = (1.f - lambda) * jps[i + 1] + lambda * jps[i];
39 jps[0] = b; // reset root to initial pos
40 for (int i = 0; i < N - 1; i++) {
41 float r = distance(jps[i + 1], jps[i]);
42 float lambda = jds[i] / r;
43 jps[i + 1] = (1 - lambda) * jps[i] + lambda * jps[i + 1];
46 float newDiff = distance(jps[N - 1], t);
47 if (newDiff == diff) abort();
54 std::vector<Model::Node> allNodesTo(const Model::Node &from, const Model::Node &to) {
55 if (from == to) return { to };
56 assert(to.parent != nullptr);
57 auto res = allNodesTo(from, *to.parent);
62 mat4 getAbsTrans(const Model::Node &root, const Model::Node &n) {
63 if (root.ai.mName == n.ai.mName)
64 return mat4(1); //aiMatrixToMat4(n.ai.mTransformation);
65 assert(n.parent != nullptr);
66 return getAbsTrans(root, *n.parent) * aiMatrixToMat4(n.ai.mTransformation);
69 vec3 extractPos(mat4 trans) {
70 return vec3(trans[3]);
73 glm::mat4 absoluteToModelSpace(const Model::Node &root, const Model::Node &n, mat4 m) {
74 const Model::Node *parent = &n;
76 std::vector<mat4> trans;
77 while (&parent->ai != &root.ai) {
78 trans.push_back(inverse(aiMatrixToMat4(parent->ai.mTransformation)));
79 parent = parent->parent;
81 while (!trans.empty()) { res = trans.back() * res; trans.pop_back(); }
85 // Given points u and v on a sphere, calculate the transformation matrix
87 // from https://math.stackexchange.com/a/2765250
88 mat4 getRotationToPoint(vec3 u, vec3 v, float dist) {
89 if (distance(u, v) < 0.00001) return mat4(1);
90 vec3 n = cross(u, v) / length(cross(u, v));
92 float alpha = atan2(dot(v, t), dot(v, u));
93 mat4 T = mat4(mat3(u, t, n));
94 mat4 R = rotate(mat4(1), alpha, {0, 0, 1}); // rotation in z axis
95 mat4 res = T * R * inverse(T);
100 void inverseKinematic(Model::Node &root, Model::Node &end, vec3 target) {
101 /* float s2o2 = sqrt(2.f) / 2.f; */
102 /* assert(getRotationToPoint({1, 0, 0}, {0, s2o2, s2o2}, 1) */
103 /* == mat4({0, s2o2, s2o2, 0}, { -s2o2, 1.f/2.f, -1.f/2.f, 0}, */
104 /* {-s2o2, -1.f/2.f, 1.f/2.f, 0}, { 0, 0, 0, 1})); */
106 std::vector<Model::Node> chain = allNodesTo(root, end);
107 assert(!chain.empty());
109 std::vector<vec3> positions(chain.size()); std::vector<float> distances(chain.size() - 1);
110 for (size_t i = 0; i < chain.size(); i++) {
111 mat4 absTrans = getAbsTrans(root, chain[i]);
112 positions[i] = extractPos(absTrans);
114 distances[i - 1] = distance(positions[i], positions[i - 1]);
117 /* glm::vec3 targetPos(sin(d * 10.f), cos(d * 10.f), 0); */
118 auto newPositions = fabrik(target, positions, distances);
120 // Rotate all the nodes so that they are in the correct positions
121 for (size_t i = 1; i < chain.size(); i++) {
122 auto node = chain[i];
123 mat4 absTrans = getAbsTrans(root, node);
124 absTrans[3] = vec4(newPositions[i], absTrans[3][3]); // update position in transform
126 vec3 oldRelPos = extractPos(aiMatrixToMat4(node.ai.mTransformation));
127 vec3 newRelPos = extractPos(absoluteToModelSpace(root, *node.parent, absTrans));
129 mat4 rot = getRotationToPoint(oldRelPos, newRelPos, distances[i - 1]);
130 node.ai.mTransformation = mat4ToaiMatrix(rot * aiMatrixToMat4(node.ai.mTransformation));
132 /* std::cerr << node.ai.mName.C_Str() << ":\n"; */
133 /* printVec3(extractPos(aiMatrixToMat4(node.ai.mTransformation))); */
134 /* printVec3(newRelPos); */
135 assert(distance(extractPos(aiMatrixToMat4(node.ai.mTransformation)), newRelPos) < 0.0001);
137 /* absTrans[3] = vec4(newPositions[i], absTrans[3][3]); // update position in transform */
139 /* mat4 relTrans = absoluteToModelSpace(root, *node.parent, absTrans); */
140 /* node.ai.mTransformation = mat4ToaiMatrix(relTrans); */
143 // TODO: Now rotate all the nodes so that they face each other
145 /* for (int i = 0; i < 3; i++) { */
146 /* glm::mat4 absTrans(1); */
147 /* findNodeTrans(&sceneModel->getRoot()->ai, aiString(jointNames[i]), */
149 /* glm::mat4 newAbsTrans = absTrans; */
150 /* newAbsTrans[3] = glm::vec4(newPositions[i], newAbsTrans[3][3]); */
152 /* auto node = sceneModel->getRoot()->ai.FindNode(jointNames[i].c_str()); */
154 /* auto newTrans = worldSpaceToModelSpace(node->mParent, newAbsTrans); */
156 /* node->mTransformation = mat4ToaiMatrix(newTrans); */