void RotateCam(int x, int y)
{
float axis_x = 0;
float axis_y = 0;
if (x != oldXM) {
if ((float)(x - oldXM) > 0)
axis_x = 1;
else
axis_x = -1;
oldXM = x;
}
if (y != oldYM) {
if ((float)(y - oldYM) > 0)
axis_y = 1;
else
axis_y = -1;
oldYM = y;
}
rot_x += axis_x*xSpeed*0.02;
rot_y += axis_y*ySpeed*0.02;
rot_x = ClampAngle(rot_x, -80, 80);
rot_y = ClampAngle(rot_y, -80, 20);
}
/**
* @brief Derives Euler angles for the specified directional vector.
*/
void VectorAngles(const vec3_t vector, vec3_t angles) {
const vec_t forward = sqrt(vector[0] * vector[0] + vector[1] * vector[1]);
vec_t pitch = ClampAngle(-Degrees(atan2(vector[2], forward)));
vec_t yaw = ClampAngle(Degrees(atan2(vector[1], vector[0])));
VectorSet(angles, pitch, yaw, 0.0);
}
void ClampAngles(Vector &angles)
{
ClampAngle(angles.x);
ClampAngle(angles.y);
angles.z = 0.0;
}
void PmxBoneSolveInverseKinematics(PMX_BONE* bone)
{
PMX_IK_CONSTRAINT *constraints = (PMX_IK_CONSTRAINT*)bone->constraints->buffer;
float root_bone_position[3];
const float angle_limit = bone->angle_limit;
const int num_constraints = (int)bone->constraints->num_data;
const int num_iteration = bone->num_iteration;
const int num_half_of_iteration = num_iteration / 2;
PMX_BONE *effector_bone = (PMX_BONE*)bone->interface_data.effector_bone;
float original_target_rotation[4];
float joint_rotation[4] = IDENTITY_QUATERNION;
float new_joint_local_rotation[4];
float matrix[9];
float local_effector_position[3] = {0};
float local_root_bone_position[3] = {0};
float local_axis[3] = {0};
int i, j, k;
if((bone->flags & PMX_BONE_FLAG_HAS_INVERSE_KINEMATICS) == 0)
{
return;
}
BtTransformGetOrigin(bone->world_transform, root_bone_position);
COPY_VECTOR4(original_target_rotation, effector_bone->local_rotation);
for(i=0; i<num_iteration; i++)
{
int perform_constraint = i < num_half_of_iteration;
for(j=0; j<num_constraints; j++)
{
PMX_IK_CONSTRAINT *constraint = &constraints[j];
PMX_BONE *joint_bone = constraint->joint_bone;
float current_effector_position[3];
void *joint_bone_transform = BtTransformCopy(joint_bone->world_transform);
void *inversed_joint_bone_transform = BtTransformCopy(joint_bone->world_transform);
float dot;
float new_angle_limit;
float angle;
float value;
BtTransformGetOrigin(effector_bone->world_transform, current_effector_position);
BtTransformInverse(inversed_joint_bone_transform);
BtTransformMultiVector3(inversed_joint_bone_transform, root_bone_position, local_root_bone_position);
Normalize3DVector(local_root_bone_position);
BtTransformMultiVector3(inversed_joint_bone_transform, current_effector_position, local_effector_position);
Normalize3DVector(local_effector_position);
dot = Dot3DVector(local_root_bone_position, local_effector_position);
if(FuzzyZero(dot) != 0)
{
break;
}
Cross3DVector(local_axis, local_effector_position, local_root_bone_position);
SafeNormalize3DVector(local_axis);
new_angle_limit = angle_limit * (j + 1) * 2;
/*if(dot < -1)
{
angle = -1;
}
else if(dot > 1)
{
angle = 1;
}
else
{
angle = acosf(dot);
}*/
value = dot;
value = ExtendedFuzzyZero(1.0f - value) ? 1.0f : ExtendedFuzzyZero(1.0f + value) ? -1.0f : value;
angle = acosf(value);
CLAMPED(angle, - new_angle_limit, new_angle_limit);
QuaternionSetRotation(joint_rotation, local_axis, angle);
if(constraint->has_angle_limit != FALSE && perform_constraint != FALSE)
{
float lower_limit[3];
float upper_limit[3];
COPY_VECTOR3(lower_limit, constraint->lower_limit);
COPY_VECTOR3(upper_limit, constraint->upper_limit);
if(i == 0)
{
if(FuzzyZero(lower_limit[1]) && FuzzyZero(upper_limit[1])
&& FuzzyZero(lower_limit[2]) && FuzzyZero(upper_limit[2]))
{
local_axis[0] = 1;
local_axis[1] = 0;
local_axis[2] = 0;
}
else if(FuzzyZero(lower_limit[0]) && FuzzyZero(upper_limit[0])
&& FuzzyZero(lower_limit[2]) && FuzzyZero(upper_limit[2]))
{
local_axis[0] = 0;
local_axis[1] = 1;
local_axis[2] = 0;
}
else if(FuzzyZero(lower_limit[0]) && FuzzyZero(upper_limit[0])
&& FuzzyZero(lower_limit[1]) && FuzzyZero(upper_limit[1]))
{
local_axis[0] = 0;
local_axis[1] = 0;
local_axis[2] = 1;
}
QuaternionSetRotation(joint_rotation, local_axis, angle);
}
else
{
float x1, y1, z1, x2, y2, z2, x3, y3, z3;
Matrix3x3SetRotation(matrix, joint_rotation);
Matrix3x3GetEulerZYX(matrix, &z1, &y1, &x1);
Matrix3x3SetRotation(matrix, joint_bone->local_rotation);
Matrix3x3GetEulerZYX(matrix, &z2, &y2, &x2);
x3 = x1 + x2, y3 = y1 + y2, z3 = z1 + z2;
x1 = ClampAngle(lower_limit[0], upper_limit[0], x3, x1);
y1 = ClampAngle(lower_limit[1], upper_limit[1], y3, y1);
z1 = ClampAngle(lower_limit[2], upper_limit[2], z3, z1);
QuaternionSetEulerZYX(joint_rotation, z1, y1, x1);
}
COPY_VECTOR4(new_joint_local_rotation, joint_rotation);
MultiQuaternion(new_joint_local_rotation, joint_bone->local_rotation);
}
else if(i == 0)
{
//COPY_VECTOR4(new_joint_local_rotation, joint_bone->local_rotation);
//MultiQuaternion(new_joint_local_rotation, joint_rotation);
COPY_VECTOR4(new_joint_local_rotation, joint_rotation);
MultiQuaternion(new_joint_local_rotation, joint_bone->local_rotation);
}
else
{
//COPY_VECTOR4(new_joint_local_rotation, joint_rotation);
//MultiQuaternion(new_joint_local_rotation, joint_bone->local_rotation);
COPY_VECTOR4(new_joint_local_rotation, joint_bone->local_rotation);
MultiQuaternion(new_joint_local_rotation, joint_rotation);
}
PmxBoneSetLocalOrientation(joint_bone, new_joint_local_rotation);
COPY_VECTOR4(joint_bone->joint_rotation, joint_rotation);
for(k=j; k>=0; k--)
{
PMX_IK_CONSTRAINT *ik = &constraints[k];
PMX_BONE *joint = ik->joint_bone;
PmxBoneUpdateWorldTransformSimple(joint);
}
PmxBoneUpdateWorldTransformSimple(effector_bone);
DeleteBtTransform(joint_bone_transform);
DeleteBtTransform(inversed_joint_bone_transform);
}
}
PmxBoneSetLocalOrientation(effector_bone, original_target_rotation);
}
