Initial works for character IK

src/modules/CharacterModule.cc

@@ -291,9 +291,6 @@ bool CharacterEntity::LoadRig(const char* filename) {
 						vi, frame_name.c_str());
 			}
 
-
-
-
 			Transform mesh_transform;
 			if (visual_table["scale"].exists()) {
 				gLog("  Warning: keyword scale not supported for visual %d of frame %s",
@@ -352,7 +349,7 @@ bool CharacterEntity::LoadRig(const char* filename) {
 
 static float cur_time = 0.0f;
 
-void CharacterEntity::Update(float dt) {
+void CharacterEntity::ApplyCharacterController(float dt) {
 	Vector3f mController_acceleration (
 			mController.mDirection[0] * cGroundAcceleration,
 			mController.mDirection[1] * cGroundAcceleration,
@@ -411,7 +408,29 @@ void CharacterEntity::Update(float dt) {
 		mAnimTime = mAnimation.mDuration * 0.125f;
 		mRigState.q.setZero();
 	}
+}
 
+void CharacterEntity::UpdateIKConstraintSet() {
+	if (mIKConstraints.size() == 0)
+		return;
+
+	UpdateIKConstraintSet();
+
+	bool result = false;
+	VectorNd q_init (mRigState.q);
+	VectorNd q_res (mRigState.q);
+	result = RigidBodyDynamics::InverseKinematics(
+			*mRigModel,
+			q_init,
+			mIKConstraintSet,
+			q_res
+			);
+	mRigState.q = q_res;
+}
+
+void CharacterEntity::Update(float dt) {
+	ApplyCharacterController(dt);
+	ApplyIKConstraints();
 	UpdateBoneMatrices();
 
 	cur_time += dt;

src/modules/CharacterModule.h

@@ -49,6 +49,13 @@ struct Animation {
 	std::vector<float> mFrameTimes;
 };
 
+struct IKConstraint {
+	bool mEnabled = true;
+	int mEffectorBodyId;
+	Vector3f mEffectorLocalOffset;
+	Vector3f mEffectorWorldTarget;
+};
+
 struct CharacterEntity {
 	/// Render entity
 	Entity *mEntity = nullptr;
@@ -63,6 +70,9 @@ struct CharacterEntity {
 	std::vector<int> mBoneFrameIndices;
 	RigState mRigState;
 
+	std::vector<IKConstraint> mIKConstraints;
+	RigidBodyDynamics::InverseKinematicsConstraintSet mIKConstraintSet;
+
 	Animation mAnimation;
 	float mAnimTime;
 
@@ -76,6 +86,10 @@ struct CharacterEntity {
 	}
 
 	bool LoadRig (const char* filename);
+	void UpdateIKConstraintSet();
+	
+	void ApplyCharacterController(float dt);
+	void ApplyIKConstraints();
 	void UpdateBoneMatrices();
 
 	void Update(float dt);