- // 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()); */
+ // 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
+ // FIXME: Despite normalizeScale, this is still numerically unstable
+ // and the transformation scales over time!!!
+ 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 nextNodePos = extractPos(aiMatrixToMat4(nextNode.ai.mTransformation));
+
+ vec3 up = {0, 1, 0};
+ vec3 dir = -normalize(nextNodePos);
+
+ 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));
+
+ // very important that we normalize the scale
+ // otherwise we end up with gradually growing models
+ // due to numerical instabaility
+ rot = normalizeScale(rot);
+ node.ai.mTransformation = mat4ToaiMatrix(oldTrans * rot);
+
+ for (auto child: node.getChildren()) {
+ child->ai.mTransformation = mat4ToaiMatrix(normalizeScale(inverse(rot)) * aiMatrixToMat4(child->ai.mTransformation));
+ }