return vec3(trans[3]);
}
-glm::mat4 absoluteToModelSpace(const Model::Node &root, const Model::Node &n, mat4 m) {
+mat4 absoluteToModelSpace(const Model::Node &root, const Model::Node &n, mat4 m) {
const Model::Node *parent = &n;
glm::mat4 res = m;
std::vector<mat4> trans;
return res;
}
+// Normalizes a transformation matrix to not have any scale factor
+inline mat4 normalizeScale(mat4 m) {
+ for (int i = 0; i < 3; i++)
+ m[i] = normalize(m[i]);
+ return m;
+}
+
float d = 0;
// Target is world position
-void inverseKinematic(Model::Node &start, Model::Node &end, vec3 target) {
+void inverseKinematics(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;
+ // Calculate the world root
+ const Model::Node &root = start.getRoot();
+ // Work out the positions and distances
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]);
}
+ // Do the actual IK part
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
- 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 */
-
- /* 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
+ // 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 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); */
+ // 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);
- 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));
+ child->ai.mTransformation = mat4ToaiMatrix(normalizeScale(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)); */
- /* } */
}
}
projects / opengl.git / blobdiff