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

diff --git a/ik.cpp b/ik.cpp
index fe4ecbb..3895efe 100644
--- a/ik.cpp
+++ b/ik.cpp
@@ -3,7 +3,7 @@
 
 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
@@ -96,16 +96,18 @@ mat4 getRotationToPoint(vec3 u, vec3 v, float dist) {
 	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]);
@@ -114,46 +116,54 @@ void inverseKinematic(Model::Node &root, Model::Node &end, vec3 target) {
 			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
+
+	
+	// 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));
+		/* 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)); */
 
-		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
 		
-		/* 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);
+		mat4 relTrans = absoluteToModelSpace(root, *node.parent, absTrans);
+		node.ai.mTransformation = mat4ToaiMatrix(relTrans);
+	}
 
-		/* absTrans[3] = vec4(newPositions[i], absTrans[3][3]); // update position in transform */
+	// 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!!!
+	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));
 
-		/* mat4 relTrans = absoluteToModelSpace(root, *node.parent, absTrans); */
-		/* node.ai.mTransformation = mat4ToaiMatrix(relTrans); */
-	}
 
-	// TODO: Now rotate all the nodes so that they face each other
+		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));
 
-	/* 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()); */
+		node.ai.mTransformation = mat4ToaiMatrix(oldTrans * rot);
 
-	/* 	auto newTrans = worldSpaceToModelSpace(node->mParent, newAbsTrans); */
+		for (auto child: node.getChildren()) {
+			child->ai.mTransformation = mat4ToaiMatrix(inverse(rot) * aiMatrixToMat4(child->ai.mTransformation));
+		}
+
+	}
 
-	/* 	node->mTransformation = mat4ToaiMatrix(newTrans); */
-	/* } */
 }