using namespace glm;
-constexpr float tolerance = 0.3;
+constexpr float tolerance = 0.001;
const std::vector<glm::vec3> fabrik(const glm::vec3 t,
const std::vector<glm::vec3> jpsIn, // joint positions
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<Model::Node> chain = allNodesTo(root, end);
+// Target is world position
+void inverseKinematic(Model::Node &start, Model::Node &end, vec3 target) {
+ std::vector<Model::Node> 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<vec3> positions(chain.size()); std::vector<float> distances(chain.size() - 1);
for (size_t i = 0; i < chain.size(); i++) {
mat4 absTrans = getAbsTrans(root, chain[i]);
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);
/* 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 */
/* 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)); */
+ /* } */
+ }
+
}