private void UpdateJointTransform(float t)
{
// FK
if (m_bvhLoaded)
{
FKIKPlugin.UpdateBVHSkeleton(m_id, t);
FKIKPlugin.GetJointData(m_id, m_jointDataArray, m_joints.Count);
}
else
{
FKIKPlugin.UpdateSkeleton(m_id);
FKIKPlugin.GetJointData(m_id, m_jointDataArray, m_joints.Count);
}
ApplyJointDataArray();
// Check foot-ground contact
CheckFootGroundContact();
// IK
if (m_enableFootIK)
{
FootIK();
}
}
private void FootIK()
{
// Get foot position in world space
Vector3 leftFootPos = m_leftFoot.transform.position;
Vector3 rightFootPos = m_rightFoot.transform.position;
/// C++ Implementation
RaycastFootHeight(leftFootPos, out float leftHeight, out Vector3 leftNormal);
RaycastFootHeight(rightFootPos, out float rightHeight, out Vector3 rightNormal);
FKIKPlugin.SolveFootIK(m_id, leftHeight, rightHeight, m_leftFootGroundContact, m_rightFootGroundContact,
FKIKPlugin.UnityToBVHTranslation(leftNormal), FKIKPlugin.UnityToBVHTranslation(rightNormal));
FKIKPlugin.GetJointData(m_id, m_jointDataArray, m_joints.Count);
ApplyJointDataArray();
/// Unity Implementation
//if (RaycastFootHeight(leftFootPos, out float leftHeight, out Vector3 leftNormal))
//{
// leftFootPos.y = Mathf.Lerp(leftHeight + leftFootPos.y, m_lastLeftFootY, m_footSpeedY);
//}
//if (RaycastFootHeight(rightFootPos, out float rightHeight, out Vector3 rightNormal))
//{
// rightFootPos.y = Mathf.Lerp(rightHeight + rightFootPos.y, m_lastRightFootY, m_footSpeedY);
//}
//m_lastLeftFootY = leftFootPos.y;
//m_lastRightFootY = rightFootPos.y;
//// Transform to guide space
//leftFootPos = gameObject.transform.InverseTransformPoint(leftFootPos); // World space to guide space
//rightFootPos = gameObject.transform.InverseTransformPoint(rightFootPos); // World space to guide space
//// Move root height
//float rootOffsetY = leftFootPos.y < rightFootPos.y ? leftHeight : rightHeight;
//rootOffsetY = Mathf.Lerp(m_lastRootOffsetY, rootOffsetY, m_rootSpeedY);
//m_lastRootOffsetY = rootOffsetY;
//Vector3 rootPos = m_root.transform.position + new Vector3(0, rootOffsetY, 0);
//rootPos = gameObject.transform.InverseTransformPoint(rootPos);
//FKIKPlugin.SetRootJointTranslation(m_id, FKIKPlugin.UnityToBVHTranslation(rootPos));
//FKIKPlugin.UpdateSkeleton(m_id);
//FKIKPlugin.SolveLimbIK(m_id, FindIdByJoint(m_leftFoot), FKIKPlugin.UnityToBVHTranslation(leftFootPos));
//FKIKPlugin.SolveLimbIK(m_id, FindIdByJoint(m_rightFoot), FKIKPlugin.UnityToBVHTranslation(rightFootPos));
//FKIKPlugin.GetJointData(m_id, m_jointDataArray, m_joints.Count);
//ApplyJointDataArray();
//// Foot rotation
//if (m_leftFootGroundContact)
//{
// Vector3 leftForward = Vector3.Cross(m_leftFoot.transform.right, leftNormal); // Calculate new forward vector
// m_leftFoot.transform.up = leftNormal;
// m_leftFoot.transform.forward = leftForward;
//}
//if (m_rightFootGroundContact)
//{
// Vector3 rightForward = Vector3.Cross(m_rightFoot.transform.right, rightNormal);
// m_rightFoot.transform.up = rightNormal;
// m_rightFoot.transform.forward = rightForward;
//}
}