ik.cpp

   1 #include "ik.hpp"
   2 #include "util.hpp"
   3
   4 using namespace glm;
   5
   6 constexpr float tolerance = 0.001;
   7
   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
  11                 ) {
  12         size_t N = jpsIn.size();
  13         assert(N == jds.size() + 1);
  14         std::vector<glm::vec3> jps = jpsIn;
  15
  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;
  24                 }
  25         } else { // target is reachable
  26                 glm::vec3 b = jps[0];
  27                 // distance between end effector and target
  28                 float diff = distance(jps[N - 1], t);
  29                 while (diff > tolerance) {
  30                         // forward reaching
  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];
  36                         }
  37
  38                         // backward reaching
  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];
  44                         }
  45
  46                         float newDiff = distance(jps[N - 1], t);
  47                         if (newDiff == diff) abort();
  48                         diff = newDiff;
  49                 }
  50         }
  51         return jps;
  52 }
  53
  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);
  58         res.push_back(to);
  59         return res;
  60 }
  61
  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);
  67 }
  68
  69 vec3 extractPos(mat4 trans) {
  70         return vec3(trans[3]);
  71 }
  72
  73 glm::mat4 absoluteToModelSpace(const Model::Node &root, const Model::Node &n, mat4 m) {
  74         const Model::Node *parent = &n;
  75         glm::mat4 res = m;
  76         std::vector<mat4> trans;
  77         while (&parent->ai != &root.ai) {
  78                 trans.push_back(inverse(aiMatrixToMat4(parent->ai.mTransformation)));
  79                 parent = parent->parent;
  80         }
  81         while (!trans.empty()) { res = trans.back() * res; trans.pop_back(); }
  82         return res;
  83 }
  84
  85 // Given points u and v on a sphere, calculate the transformation matrix
  86 // to bring u -> v
  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));
  91         vec3 t = cross(n, u);
  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);
  96         return res;
  97 }
  98
  99 float d = 0;
 100
 101 // Target is world position
 102 void inverseKinematic(Model::Node &start, Model::Node &end, vec3 target) {
 103         std::vector<Model::Node> chain = allNodesTo(start, end);
 104         assert(!chain.empty());
 105
 106         const Model::Node *rootPtr = start.parent;
 107         while (rootPtr->parent != nullptr)
 108                 rootPtr = rootPtr->parent;
 109         const Model::Node &root = *rootPtr;
 110
 111         std::vector<vec3> positions(chain.size()); std::vector<float> distances(chain.size() - 1);
 112         for (size_t i = 0; i < chain.size(); i++) {
 113                 mat4 absTrans = getAbsTrans(root, chain[i]);
 114                 positions[i] = extractPos(absTrans);
 115                 if (i > 0)
 116                         distances[i - 1] = distance(positions[i], positions[i - 1]);
 117         }
 118
 119         auto newPositions = fabrik(target, positions, distances);
 120
 121         // Rotate all the nodes so that they are in the correct positions
 122         // Don't need to move the root node - it's already in place
 123         for (size_t i = 1; i < chain.size(); i++) {
 124                 auto node = chain[i];
 125                 mat4 absTrans = getAbsTrans(root, node);
 126                 absTrans[3] = vec4(newPositions[i], absTrans[3][3]); // update position in transform
 127
 128                 vec3 oldRelPos = extractPos(aiMatrixToMat4(node.ai.mTransformation));
 129                 vec3 newRelPos = extractPos(absoluteToModelSpace(root, *node.parent, absTrans));
 130
 131                 mat4 rot = getRotationToPoint(oldRelPos, newRelPos, distances[i - 1]);
 132                 node.ai.mTransformation = mat4ToaiMatrix(rot * aiMatrixToMat4(node.ai.mTransformation));
 133
 134                 /* std::cerr << node.ai.mName.C_Str() << ":\n"; */
 135                 /* printVec3(extractPos(aiMatrixToMat4(node.ai.mTransformation))); */
 136                 /* printVec3(newRelPos); */
 137                 /* assert(distance(extractPos(aiMatrixToMat4(node.ai.mTransformation)), newRelPos) < 0.0001); */
 138
 139                 /* absTrans[3] = vec4(newPositions[i], absTrans[3][3]); // update position in transform */
 140
 141                 /* mat4 relTrans = absoluteToModelSpace(root, *node.parent, absTrans); */
 142                 /* node.ai.mTransformation = mat4ToaiMatrix(relTrans); */
 143         }
 144
 145         // TODO: Now rotate all the nodes so that they point towards each other
 146         // Don't need to rotate the last node - it has nothing to point towards
 147         for (size_t i = 0; i < chain.size() - 1; i++) {
 148                 auto node = chain[i]; auto nextNode = chain[i + 1];
 149                 mat4 oldTrans = aiMatrixToMat4(node.ai.mTransformation);
 150                 vec3 nodePos = extractPos(oldTrans);
 151                 vec3 nextNodePos = extractPos(aiMatrixToMat4(nextNode.ai.mTransformation));
 152
 153
 154                 /* vec3 up = normalize(oldTrans[1]); */
 155                 vec3 up = {0, 1, 0};
 156                 vec3 dir = -normalize(nextNodePos);
 157                 /* mat4 rot = mat3(aiMatrixToMat4(nextNode.ai.mTransformation)); */
 158                 /* mat4 rot = transpose(lookAt(vec3(0), dir, {0, 1, 0})); */
 159
 160                 vec3 v = cross(up, dir);
 161                 mat3 sscpm = mat3(0, -v[2], v[1],
 162                                                   v[2], 0, -v[0],
 163                                                   -v[1], v[0], 0);
 164                 mat4 rot = mat3(1) + sscpm + sscpm * sscpm * (1.f / 1.f + dot(up, dir));
 165                 /* mat4 rot = rotate(mat4(1), 0.01f, {0, 1, 0}); */
 166                 /* mat4 rot = mat4(1); */
 167
 168
 169                 mat4 trans = oldTrans * rot * inverse(oldTrans);
 170                 node.ai.mTransformation = mat4ToaiMatrix(trans * oldTrans);
 171                 for (auto child: node.getChildren()) {
 172                         child->ai.mTransformation = mat4ToaiMatrix(inverse(rot) * aiMatrixToMat4(child->ai.mTransformation));
 173                 }
 174
 175
 176                 /* vec3 nextNodePos = extractPos(oldTrans * aiMatrixToMat4(nextNode.ai.mTransformation)); */
 177                 /* vec3 d = normalize(nextNodePos - nodePos); */
 178
 179                 /* mat4 m = mat3(d.z, 0, -d.x, 0, 1, 0, d.x, 0, d.z); */
 180
 181                 /* vec3 up = oldTrans[1]; */
 182                 /* mat4 look = lookAt(vec3(0), nextNodePos, up); */
 183                 /* look = oldTrans * look * inverse(oldTrans); */
 184                 /* node.ai.mTransformation = mat4ToaiMatrix(m * oldTrans); */
 185
 186                 /* for (auto child: node.getChildren()) { */
 187                 /*      child->ai.mTransformation = mat4ToaiMatrix(inverse(m) * aiMatrixToMat4(child->ai.mTransformation)); */
 188                 /* } */
 189
 190                 /* vec3 pos = extractPos(oldTrans); */
 191                 /* vec3 up = normalize(translate(oldTrans, -pos)); */
 192                 /* vec3 up = oldTrans[1]; */
 193                 /* mat4 rot = lookAt(vec3(0), extractPos(aiMatrixToMat4(nextNode.ai.mTransformation)), up); */
 194                 /* node.ai.mTransformation = mat4ToaiMatrix(translate(rot * translate(oldTrans, -pos), pos)); */
 195                 /* for (auto child: node.getChildren()) { */
 196                 /*      child->ai.mTransformation = mat4ToaiMatrix(inverse(rot) * aiMatrixToMat4(child->ai.mTransformation)); */
 197                 /* } */
 198         }
 199
 200 }
 201