Some tidying up

diff --git a/ik.cpp b/ik.cpp
index c6fa6030748bbfb4740b5099d872651d57451874..3895efe7819abdf8a546691f5b436645fd1ac02e 100644 (file)
--- a/ik.cpp
+++ b/ik.cpp
@@ -118,83 +118,51 @@ void inverseKinematic(Model::Node &start, Model::Node &end, vec3 target) {
 
        auto newPositions = fabrik(target, positions, distances);
 
 
        auto newPositions = fabrik(target, positions, distances);
 

-       // Rotate all the nodes so that they are in the correct positions
+
+       
+       // Move 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);
                absTrans[3] = vec4(newPositions[i], absTrans[3][3]); // update position in transform
 
        // 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);
                absTrans[3] = vec4(newPositions[i], absTrans[3][3]); // update position in transform
 

-               vec3 oldRelPos = extractPos(aiMatrixToMat4(node.ai.mTransformation));
-               vec3 newRelPos = extractPos(absoluteToModelSpace(root, *node.parent, absTrans));
-
-               mat4 rot = getRotationToPoint(oldRelPos, newRelPos, distances[i - 1]);
-               node.ai.mTransformation = mat4ToaiMatrix(rot * aiMatrixToMat4(node.ai.mTransformation));
+               /* vec3 oldRelPos = extractPos(aiMatrixToMat4(node.ai.mTransformation)); */
+               /* vec3 newRelPos = extractPos(absoluteToModelSpace(root, *node.parent, absTrans)); */
+               /* mat4 rot = getRotationToPoint(oldRelPos, newRelPos, distances[i - 1]); */
+               /* node.ai.mTransformation = mat4ToaiMatrix(rot * aiMatrixToMat4(node.ai.mTransformation)); */

 
 

-               /* 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); */
-
-               /* absTrans[3] = vec4(newPositions[i], absTrans[3][3]); // update position in transform */
+               absTrans[3] = vec4(newPositions[i], absTrans[3][3]); // update position in transform

                
                

-               /* mat4 relTrans = absoluteToModelSpace(root, *node.parent, absTrans); */
-               /* node.ai.mTransformation = mat4ToaiMatrix(relTrans); */
+               mat4 relTrans = absoluteToModelSpace(root, *node.parent, absTrans);
+               node.ai.mTransformation = mat4ToaiMatrix(relTrans);

        }
 
        }
 

-       // TODO: Now rotate all the nodes so that they point towards each other
+       // 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
        // Don't need to rotate the last node - it has nothing to point towards

+       // FIXME: This is 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);
        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 nextNodePos = extractPos(aiMatrixToMat4(nextNode.ai.mTransformation));
 
 

-               /* vec3 up = normalize(oldTrans[1]); */

                vec3 up = {0, 1, 0};
                vec3 dir = -normalize(nextNodePos);
                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));
 
                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);
+               node.ai.mTransformation = mat4ToaiMatrix(oldTrans * rot);
+

                for (auto child: node.getChildren()) {
                        child->ai.mTransformation = mat4ToaiMatrix(inverse(rot) * aiMatrixToMat4(child->ai.mTransformation));
                }
 
                for (auto child: node.getChildren()) {
                        child->ai.mTransformation = mat4ToaiMatrix(inverse(rot) * aiMatrixToMat4(child->ai.mTransformation));
                }
 

-               
-               /* vec3 nextNodePos = extractPos(oldTrans * aiMatrixToMat4(nextNode.ai.mTransformation)); */
-               /* vec3 d = normalize(nextNodePos - nodePos); */
-
-               /* mat4 m = mat3(d.z, 0, -d.x, 0, 1, 0, d.x, 0, d.z); */
-
-               /* vec3 up = oldTrans[1]; */
-               /* mat4 look = lookAt(vec3(0), nextNodePos, up); */
-               /* look = oldTrans * look * inverse(oldTrans); */
-               /* node.ai.mTransformation = mat4ToaiMatrix(m * oldTrans); */
-
-               /* 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)); */
-               /* } */

        }
 
 }
        }
 
 }