Refactored IK, added gizmo to edit IK target, disabled IK for now

src/modules/CharacterModule.cc

@@ -160,6 +160,11 @@ CharacterEntity::CharacterEntity() {
 	load_result = mAnimation.Load(cAnimFile);
 	assert (load_result);
 
+	gLog ("Initializing IK constraints");
+	IKConstraint constraint;
+	constraint.mEffectorBodyId = mRigModel->GetBodyId("FootRight");
+	mIKConstraints.push_back(constraint);
+
 	mAnimTime = 0.0f;
 }
 
@@ -410,7 +415,60 @@ void CharacterEntity::ApplyCharacterController(float dt) {
 	}
 }
 
+void CharacterEntity::UpdateIKGizmos() {
+	for (int i = 0; i < mIKConstraints.size(); ++i) {
+		IKConstraint& constraint = mIKConstraints[i];
+		Transform ik_handle_transform;
+		ik_handle_transform.translation = constraint.mEffectorWorldTarget;
+		Matrix44f ent_transform = ik_handle_transform.toMatrix(); 
+
+		Camera *active_camera = &gRenderer->cameras[gRenderer->activeCameraIndex];
+
+		ImGuizmo::Manipulate(
+				active_camera->mtxView,
+				active_camera->mtxProj,
+				ImGuizmo::TRANSLATE,
+				ImGuizmo::LOCAL,
+				ent_transform.data()
+				);
+
+		ik_handle_transform.fromMatrix(ent_transform);
+		constraint.mEffectorWorldTarget = ik_handle_transform.translation;
+	}
+}
+
 void CharacterEntity::UpdateIKConstraintSet() {
+	if (mIKConstraints.size() * 3 != mIKConstraintSet.num_constraints) {
+		mIKConstraintSet.ClearConstraints();
+		for (int i = 0; i < mIKConstraints.size(); ++i) {
+			const IKConstraint& constraint = mIKConstraints[i];
+			mIKConstraintSet.AddPointConstraint(
+					constraint.mEffectorBodyId,
+					constraint.mEffectorLocalOffset,
+					constraint.mEffectorWorldTarget
+					);
+		}
+
+		mIKConstraintSet.lambda = 1.0e-3;
+		mIKConstraintSet.max_steps = 2;
+		mIKConstraintSet.step_tol = 1.0e-5;
+
+		mIKConstraintSet.Bind(*mRigModel);
+	} else {
+		for (int i = 0; i < mIKConstraints.size(); ++i) {
+			const IKConstraint& constraint = mIKConstraints[i];
+			mIKConstraintSet.body_ids[i] = constraint.mEffectorBodyId;
+			mIKConstraintSet.body_points[i] = constraint.mEffectorLocalOffset;
+			mIKConstraintSet.target_positions[i] = Vector3d (
+					constraint.mEffectorWorldTarget[0],
+					-constraint.mEffectorWorldTarget[2],
+					constraint.mEffectorWorldTarget[1]
+					);
+		}
+	}
+}
+
+void CharacterEntity::ApplyIKConstraints() {
 	if (mIKConstraints.size() == 0)
 		return;
 
@@ -426,9 +484,42 @@ void CharacterEntity::UpdateIKConstraintSet() {
 			q_res
 			);
 	mRigState.q = q_res;
+
+	for (int i = 0; i < mIKConstraints.size(); ++i) {
+		const IKConstraint& constraint = mIKConstraints[i];
+		UpdateKinematicsCustom (
+				*mRigModel, &q_res, nullptr, nullptr);
+
+		Vector3f effector_pos = CalcBodyToBaseCoordinates(
+				*mRigModel, 
+				q_res, 
+				constraint.mEffectorBodyId, 
+				constraint.mEffectorLocalOffset,
+				false
+				);
+
+		gRenderer->drawDebugSphere (
+				effector_pos,
+				0.05f,
+				Vector4f (0.f, 1.f, 0.f, 1.0f)
+				);
+
+		gRenderer->drawDebugSphere (
+				constraint.mEffectorWorldTarget,
+				0.05f,
+				Vector4f (1.f, 0.f, 0.f, 1.0f)
+				);
+
+		gRenderer->drawDebugLine (
+				effector_pos,
+				constraint.mEffectorWorldTarget,
+				Vector3f (1.f, 0.f, 1.f)
+				);
+	}
 }
 
 void CharacterEntity::Update(float dt) {
+	UpdateIKGizmos();
 	ApplyCharacterController(dt);
 	ApplyIKConstraints();
 	UpdateBoneMatrices();
@@ -553,7 +644,37 @@ void ShowCharacterPropertiesWindow (CharacterEntity* character) {
 			}
 			ImGui::TreePop();
 		}
+
+		node_open = ImGui::TreeNodeEx(
+				"IK Effectors",
+				0);
+
+		if (node_open) {
+			for (int i = 0; i < character->mIKConstraints.size(); ++i) {
+				IKConstraint& constraint = character->mIKConstraints[i];
+
+				char buf[32];
+				snprintf (buf, 32, "Constraint %d", i);
+
+				ImGuiTreeNodeFlags node_flags = 0;
+
+				bool node_open = ImGui::TreeNodeEx(
+						buf,
+						node_flags);
+
+				if (node_open) {
+					Transform &transform = character->mEntity->mSkeleton.mLocalTransforms[i];
+
+					ImGui::DragFloat3 ("Local Offset", constraint.mEffectorLocalOffset.data(), 0.01, 0.001f, 10.0f);
+					ImGui::DragFloat3 ("Target", constraint.mEffectorWorldTarget.data(), 0.01, -10.0f, 10.0f);
+
+					ImGui::TreePop();
 				}
+			}
+			ImGui::TreePop();
+		}
+	}
+
 
 	ImGui::EndDock();
 }

src/modules/CharacterModule.h

@@ -50,10 +50,10 @@ struct Animation {
 };
 
 struct IKConstraint {
-	bool mEnabled = true;
+	bool mKeepRootFixed = true;
-	int mEffectorBodyId;
+	int mEffectorBodyId = 0;
-	Vector3f mEffectorLocalOffset;
+	Vector3f mEffectorLocalOffset = Vector3f::Zero();
-	Vector3f mEffectorWorldTarget;
+	Vector3f mEffectorWorldTarget = Vector3f::Zero();
 };
 
 struct CharacterEntity {
@@ -86,10 +86,13 @@ struct CharacterEntity {
 	}
 
 	bool LoadRig (const char* filename);
-	void UpdateIKConstraintSet();
 
 	void ApplyCharacterController(float dt);
+	
+	void UpdateIKGizmos();
+	void UpdateIKConstraintSet();
 	void ApplyIKConstraints();
+
 	void UpdateBoneMatrices();
 
 	void Update(float dt); 

src/modules/RenderModule.cc

@@ -1277,32 +1277,6 @@ void Renderer::resize (int x, int y, int width, int height) {
 	}
 }
 
-void Renderer::paintGLSimple() {
-	// Set view 0 default viewport.
-	bgfx::setViewRect(0, view_offset_x, view_offset_y, view_width, view_height);
-
-	// This dummy draw call is here to make sure that view 0 is cleared
-	// if no other draw calls are submitted to view 0.
-	bgfx::touch(0);
-
-	int64_t now = bx::getHPCounter();
-	static int64_t last = now;
-	const int64_t frameTime = now - last;
-	last = now;
-	const double freq = double(bx::getHPFrequency() );
-	const double toMs = 1000.0/freq;
-
-	// Use debug font to print information about this example.
-	bgfx::dbgTextPrintf(0, 1, 0x4f, "bgfx/examples/00-helloworld");
-	bgfx::dbgTextPrintf(0, 2, 0x6f, "Description: Initialization and debug text.");
-	bgfx::dbgTextPrintf(0, 3, 0x8f, "Frame: % 7.3f[ms]", double(frameTime)*toMs);
-
-	// Advance to next frame. Rendering thread will be kicked to
-	// process submitted rendering primitives.
-	bgfx::frame();
-	bgfx::dbgTextClear();
-}
-
 void Renderer::paintGL() {
 	int64_t now = bx::getHPCounter();
 	static int64_t last = now;
@@ -1562,21 +1536,6 @@ void Renderer::paintGL() {
 		}
 	}
 
-	for (size_t i = 0; i < entities.size(); i++) {
-		Matrix44f ent_transform = entities[i]->mTransform.toMatrix();
-		Matrix44f delta_matrix;
-		ImGuizmo::Manipulate(
-				cameras[activeCameraIndex].mtxView,
-				cameras[activeCameraIndex].mtxProj,
-				ImGuizmo::TRANSLATE,
-				ImGuizmo::LOCAL,
-				ent_transform.data(),
-				delta_matrix.data()
-				);
-
-		entities[i]->mTransform.fromMatrix(ent_transform);
-	}
-
 	// render debug information
 	if (drawDebug) {
 		float tmp[16];

src/modules/RenderModule.h

@@ -412,7 +412,6 @@ struct Renderer {
 	void initialize(int width, int height);
 	void shutdown();
 	void paintGL();
-	void paintGLSimple();
 	void resize (int x, int y, int width, int height);
 
 	// check whether shader files were modified and reload them. Returns