Bone_Animation.cpp

#include "Bone_Animation.h"
#include <array>
#include <cmath>
#include <glm/gtc/type_ptr.hpp>
#include <glm/gtx/transform.hpp>

Bone_Animation::Bone_Animation()
{
}


Bone_Animation::~Bone_Animation()
{
}

void Bone_Animation::init()
{
	root_position = { 2.0f,1.0f,2.0f };
	bone1_position = { 2.0f, 3.5f, 2.0f };
	bone2_position = { 2.0f, 7.0f, 2.0f };
	bone3_position = { 2.0f, 9.5f, 2.0f };
	end_effector_position = { 0.0f, 0.0f, 0.0f };
	target_position = { 3.0f, 8.0f, 3.0f };

	scale_vector =
	{
		{1.0f,1.0f,1.0f},
		{0.5f,4.0f,0.5f},
		{0.5f,3.0f,0.5f},
		{0.5f,2.0f,0.5f}
	};

	rotation_degree_vector = 
	{
		{0.0f,0.0f,0.0f},
		{0.0f,0.0f,0.0f},
		{0.0f,0.0f,0.0f},
		{0.0f,0.0f,0.0f}
	};

	colors = 
	{
		{0.7f,0.0f,0.0f,1.0f},
		{0.7f,0.7f,0.0f,1.0f},
		{0.7f,0.0f,0.7f,1.0f},
		{0.0f,0.7f,0.7f,1.0f}
	};

}

void Bone_Animation::update(float delta_time)
{
	rotate_matrix = std::vector<glm::mat4>();

	//Root Bone (identity)
	rotate_matrix.push_back(glm::mat4(1.0f));

	// Purple Bone (bone index 1)
	{
		glm::mat4 rotX = glm::rotate(glm::mat4(1.0f), glm::radians(rotation_degree_vector[1][0]), glm::vec3(1.0f, 0.0f, 0.0f));
		glm::mat4 rotY = glm::rotate(glm::mat4(1.0f), glm::radians(rotation_degree_vector[1][1]), glm::vec3(0.0f, 1.0f, 0.0f));
		glm::mat4 rotZ = glm::rotate(glm::mat4(1.0f), glm::radians(rotation_degree_vector[1][2]), glm::vec3(0.0f, 0.0f, 1.0f));
		glm::mat4 purpleMatrix = rotX * rotY * rotZ;
		rotate_matrix.push_back(purpleMatrix);
	}

	// Cyan Bone
	{
		glm::mat4 rotX = glm::rotate(glm::mat4(1.0f), glm::radians(rotation_degree_vector[2][0]), glm::vec3(1.0f, 0.0f, 0.0f));
		glm::mat4 rotY = glm::rotate(glm::mat4(1.0f), glm::radians(rotation_degree_vector[2][1]), glm::vec3(0.0f, 1.0f, 0.0f));
		glm::mat4 rotZ = glm::rotate(glm::mat4(1.0f), glm::radians(rotation_degree_vector[2][2]), glm::vec3(0.0f, 0.0f, 1.0f));
		glm::mat4 cyanMatrix = rotX * rotY * rotZ;
		rotate_matrix.push_back(cyanMatrix);
	}

	// Green Bone
	{
		glm::mat4 rotX = glm::rotate(glm::mat4(1.0f), glm::radians(rotation_degree_vector[3][0]), glm::vec3(1.0f, 0.0f, 0.0f));
		glm::mat4 rotY = glm::rotate(glm::mat4(1.0f), glm::radians(rotation_degree_vector[3][1]), glm::vec3(0.0f, 1.0f, 0.0f));
		glm::mat4 rotZ = glm::rotate(glm::mat4(1.0f), glm::radians(rotation_degree_vector[3][2]), glm::vec3(0.0f, 0.0f, 1.0f));
		glm::mat4 greenMatrix = rotX * rotY * rotZ;
		rotate_matrix.push_back(greenMatrix);
	}
}

glm::mat4 Bone_Animation::bone1_matrix(glm::mat4 bone1_mat)
{
	bone1_mat =
		glm::translate(bone1_mat, glm::vec3(0.0f, -(scale_vector[1][1] / 2), 0.0f)) *		//move bone origin to bottom of bone from center of bone
		glm::translate(bone1_mat, bone1_position) *											//move bone according to root_position
		rotate_matrix[1] *																	//rotate bone according to rotation_degree_vector
		glm::translate(bone1_mat, glm::vec3(0.0f, scale_vector[1][1] / 2, 0.0f));			//move bone origin back to center of bone

	bone1_mat = glm::translate(bone1_mat, glm::vec3(0.0f, scale_vector[1][1] / 2, 0.0f));	//move to bone end
	bone1_end = bone1_mat[3];																//Assign bone1_end
	bone1_mat = glm::translate(bone1_mat, glm::vec3(0.0f, -(scale_vector[1][1] / 2), 0.0f));//undo move to end
	bone1_mat = glm::scale(bone1_mat, scale_vector[1]);										//scale bone according to scale_vector

	bone1Matrix = bone1_mat;
	return bone1_mat;
}

glm::mat4 Bone_Animation::bone2_matrix(glm::mat4 bone2_mat)
{
	bone2_mat =
		glm::translate(bone2_mat, glm::vec3(0.0f, -(scale_vector[2][1] / 2), 0.0f)) *		//move bone origin to bottom of bone from center of bone
		glm::translate(bone2_mat, glm::vec3(0.0f, -(scale_vector[1][1]), 0.0f)) *				//Apply bone1 -translation
		glm::translate(bone2_mat, bone2_position) *											//move bone according to root_position
		rotate_matrix[1] *																		//Apply bone1 rotation
		glm::translate(bone2_mat, glm::vec3(0.0f, scale_vector[1][1], 0.0f)) *					//Apply bone1 translation
		rotate_matrix[2] *																	//rotate bone according to rotation_degree_vector
		glm::translate(bone2_mat, glm::vec3(0.0f, scale_vector[2][1] / 2, 0.0f));			//move bone origin back to center of bone

	bone2_mat = glm::translate(bone2_mat, glm::vec3(0.0f, scale_vector[2][1] / 2, 0.0f));	//move to bone end
	bone2_end = bone2_mat[3];																//Assign bone2_end
	bone2_mat = glm::translate(bone2_mat, glm::vec3(0.0f, -(scale_vector[2][1] / 2), 0.0f));//undo move to end
	bone2_mat = glm::scale(bone2_mat, scale_vector[2]);										//scale bone according to scale_vector

	bone2Matrix = bone2_mat;
	return bone2_mat;
}

glm::mat4 Bone_Animation::bone3_matrix(glm::mat4 bone3_mat)
{
	bone3_mat =
		glm::translate(bone3_mat, glm::vec3(0.0f, -(scale_vector[3][1] / 2), 0.0f)) *			//move bone origin to bottom of bone from center of bone
		glm::translate(bone3_mat, glm::vec3(0.0f, -(scale_vector[2][1]), 0.0f)) *				//Apply bone2 -translation
		glm::translate(bone3_mat, glm::vec3(0.0f, -(scale_vector[1][1]), 0.0f)) *				//Apply bone1 -translation
		glm::translate(bone3_mat, bone3_position) *												//move bone according to root_position
		rotate_matrix[1] *																		//Apply bone1 rotation
		glm::translate(bone3_mat, glm::vec3(0.0f, scale_vector[1][1], 0.0f)) *					//Apply bone1 translation
		rotate_matrix[2] *																		//Apply bone2 rotation
		glm::translate(bone3_mat, glm::vec3(0.0f, scale_vector[2][1], 0.0f)) *					//Apply bone2 translation
		rotate_matrix[3] *																		//rotate bone according to rotation_degree_vector
		glm::translate(bone3_mat, glm::vec3(0.0f, scale_vector[3][1] / 2, 0.0f));				//move bone origin back to center of bone

	bone3_mat = glm::translate(bone3_mat, glm::vec3(0.0f, scale_vector[3][1] / 2, 0.0f));		//move to bone end
	end_effector_position = bone3_mat[3];														//Assign bone3's center position to endEffector's position
	bone3_mat = glm::translate(bone3_mat, glm::vec3(0.0f, -(scale_vector[3][1] / 2), 0.0f));	//undo move to end
	bone3_mat = glm::scale(bone3_mat, scale_vector[3]);											//scale bone according to scale_vector

	bone3Matrix = bone3_mat;
	return bone3_mat;
}

bool Bone_Animation::jacobian_IK()
{
	// Stop if end effector is close enough
	const float eps_dist = 0.1f;
	if (glm::length(end_effector_position - target_position) <= eps_dist)
		return false;

	// Build jacobian vectors (must be up-to-date)
	get_jac_orientation();

	// Goal vector (3x1)
	glm::vec3 goal_vec = target_position - end_effector_position;

	// Build 3x3 matrix A = J * J^T  (J: 3x9, columns are jacobian_vectors[k])
	glm::mat3 A(0.0f);
	for (size_t k = 0; k < jacobian_vectors.size(); ++k) {
		glm::vec3 c = jacobian_vectors[k];
		// outer product c * c^T
		A += glm::mat3(
			c.x * c.x, c.x * c.y, c.x * c.z,
			c.y * c.x, c.y * c.y, c.y * c.z,
			c.z * c.x, c.z * c.y, c.z * c.z
		);
	}

	// Damped Least Squares: solve x = (A + lambda^2 I)^-1 * goal_vec
	float lambda = dls_lambda;
	glm::mat3 B = A + glm::mat3(lambda * lambda);
	// guard against singular matrix
	float detB = glm::determinant(B);
	const float det_eps = 1e-8f;
	if (fabs(detB) < det_eps) {
		// increase damping to regularize
		B += glm::mat3( (lambda + 1e-2f) * (lambda + 1e-2f) );
	}

	glm::mat3 invB = glm::inverse(B);
	glm::vec3 x = invB * goal_vec; // 3x1

	// Δθ = J^T * x  -> for each joint k: delta_k = dot(jacobian_vectors[k], x)
	// Apply ik_speed_multiplier to scale the change
	for (size_t k = 0; k < jacobian_vectors.size() && k < 9; ++k)
	{
		float delta_k = glm::dot(jacobian_vectors[k], x) * ik_speed_multiplier;
		int boneIdx = static_cast<int>(k / 3) + 1; // bones 1..3
		int compIdx = static_cast<int>(k % 3);     // component 0..2 corresponds to (X,Y,Z) in rotation_degree_vector
		rotation_degree_vector[boneIdx][compIdx] += delta_k;
	}

	return true;
}

void Bone_Animation::get_jac_orientation()
{
	glm::vec3 root_top = { root_position[0], root_position[1] + (scale_vector[0][1] / 2), root_position[2] };
	glm::vec3 vec1 = glm::cross(glm::vec3(bone1Matrix[0]), end_effector_position - root_top);
	glm::vec3 vec2 = glm::cross(glm::vec3(bone1Matrix[1]), end_effector_position - root_top);
	glm::vec3 vec3 = glm::cross(glm::vec3(bone1Matrix[2]), end_effector_position - root_top);
	glm::vec3 vec4 = glm::cross(glm::vec3(bone2Matrix[0]), end_effector_position - bone1_end);
	glm::vec3 vec5 = glm::cross(glm::vec3(bone2Matrix[1]), end_effector_position - bone1_end);
	glm::vec3 vec6 = glm::cross(glm::vec3(bone2Matrix[2]), end_effector_position - bone1_end);
	glm::vec3 vec7 = glm::cross(glm::vec3(bone3Matrix[0]), end_effector_position - bone2_end);
	glm::vec3 vec8 = glm::cross(glm::vec3(bone3Matrix[1]), end_effector_position - bone2_end);
	glm::vec3 vec9 = glm::cross(glm::vec3(bone3Matrix[2]), end_effector_position - bone2_end);

	jacobian_vectors =
	{
		vec1,
		vec2,
		vec3,
		vec4,
		vec5,
		vec6,
		vec7,
		vec8,
		vec9
	};
}

void Bone_Animation::reset()
{
	rotation_degree_vector =
	{
		{0.0f,0.0f,0.0f},
		{0.0f,0.0f,0.0f},
		{0.0f,0.0f,0.0f},
		{0.0f,0.0f,0.0f}
	};
}