6 constexpr float tolerance = 0.001;
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 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);
99 // Normalizes a transformation matrix to not have any scale factor
100 inline mat4 normalizeScale(mat4 m) {
101 for (int i = 0; i < 3; i++)
102 m[i] = normalize(m[i]);
108 // Target is world position
109 void inverseKinematics(Model::Node &start, Model::Node &end, vec3 target) {
110 std::vector<Model::Node> chain = allNodesTo(start, end);
111 assert(!chain.empty());
113 // Calculate the world root
114 const Model::Node &root = start.getRoot();
116 // Work out the positions and distances
117 std::vector<vec3> positions(chain.size()); std::vector<float> distances(chain.size() - 1);
118 for (size_t i = 0; i < chain.size(); i++) {
119 mat4 absTrans = getAbsTrans(root, chain[i]);
120 positions[i] = extractPos(absTrans);
122 distances[i - 1] = distance(positions[i], positions[i - 1]);
125 // Do the actual IK part
126 auto newPositions = fabrik(target, positions, distances);
128 // Move all the nodes so that they are in the correct positions
129 // Don't need to move the root node - it's already in place
130 for (size_t i = 1; i < chain.size(); i++) {
131 auto node = chain[i];
132 mat4 absTrans = getAbsTrans(root, node);
133 absTrans[3] = vec4(newPositions[i], absTrans[3][3]); // update position in transform
135 mat4 relTrans = absoluteToModelSpace(root, *node.parent, absTrans);
136 node.ai.mTransformation = mat4ToaiMatrix(relTrans);
139 // Now rotate all the nodes so that they point towards each other
140 // Don't need to rotate the last node - it has nothing to point towards
141 // FIXME: Despite normalizeScale, this is still numerically unstable
142 // and the transformation scales over time!!!
143 for (size_t i = 0; i < chain.size() - 1; i++) {
144 auto node = chain[i]; auto nextNode = chain[i + 1];
145 mat4 oldTrans = aiMatrixToMat4(node.ai.mTransformation);
146 vec3 nextNodePos = extractPos(aiMatrixToMat4(nextNode.ai.mTransformation));
149 vec3 dir = -normalize(nextNodePos);
151 vec3 v = cross(up, dir);
152 mat3 sscpm = mat3(0, -v[2], v[1],
155 mat4 rot = mat3(1) + sscpm + sscpm * sscpm * (1.f / 1.f + dot(up, dir));
157 // very important that we normalize the scale
158 // otherwise we end up with gradually growing models
159 // due to numerical instabaility
160 rot = normalizeScale(rot);
161 node.ai.mTransformation = mat4ToaiMatrix(oldTrans * rot);
163 for (auto child: node.getChildren()) {
164 child->ai.mTransformation = mat4ToaiMatrix(normalizeScale(inverse(rot)) * aiMatrixToMat4(child->ai.mTransformation));