X-Git-Url: https://git.lukelau.me/?p=opengl.git;a=blobdiff_plain;f=ik.cpp;fp=ik.cpp;h=9598ddc870ee0d45b3d8d322ba87aa696d4f3391;hp=3895efe7819abdf8a546691f5b436645fd1ac02e;hb=ad77a970429c9c5e9cf8d513d0469d03ad82e622;hpb=671418e7effc7cbbb7e05df216291406d2b82dd7

diff --git a/ik.cpp b/ik.cpp
index 3895efe..9598ddc 100644
--- a/ik.cpp
+++ b/ik.cpp
@@ -70,7 +70,7 @@ vec3 extractPos(mat4 trans) {
 	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;
@@ -96,18 +96,24 @@ mat4 getRotationToPoint(vec3 u, vec3 v, float dist) {
 	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]);
@@ -116,10 +122,9 @@ void inverseKinematic(Model::Node &start, Model::Node &end, vec3 target) {
 			distances[i - 1] = distance(positions[i], positions[i - 1]);
 	}
 
+	// Do the actual IK part
 	auto newPositions = fabrik(target, positions, distances);
 	
-
-	
 	// 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++) {
@@ -127,26 +132,19 @@ void inverseKinematic(Model::Node &start, Model::Node &end, vec3 target) {
 		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)); */
-
-		absTrans[3] = vec4(newPositions[i], absTrans[3][3]); // update position in transform
-		
 		mat4 relTrans = absoluteToModelSpace(root, *node.parent, absTrans);
 		node.ai.mTransformation = mat4ToaiMatrix(relTrans);
 	}
 
 	// 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: This is numerically unstable and the transformation scales over time!!!
+	// 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);
 
@@ -156,11 +154,14 @@ void inverseKinematic(Model::Node &start, Model::Node &end, vec3 target) {
 						  -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(inverse(rot) * aiMatrixToMat4(child->ai.mTransformation));
+			child->ai.mTransformation = mat4ToaiMatrix(normalizeScale(inverse(rot)) * aiMatrixToMat4(child->ai.mTransformation));
 		}
 
 	}