internal Point3D GetMorphedWorldPosition(int boneIndex, VpdPose vpdPose)
{
var bone = bones[boneIndex];
var result = new Point3D(0, 0, 0);
Matrix3D m;
while (bone != null)
{
if (vpdPose.JointChangeByBoneName.ContainsKey(bone.Name))
{
var jointChange = vpdPose.GetRightHandedJointChange(bone.Name);
m = Matrix3D.Identity;
m.Rotate(jointChange.Orientation);
result = m.Transform(result);
result += jointChange.Position;
}
m = Matrix3D.Identity;
m.Scale(new Vector3D(1, 1, -1));
result += m.Transform(GetBoneDisplacementFromParent(boneIndex));
if (bone.ParentIndex >= 0)
{
boneIndex = bone.ParentIndex;
bone = bones[bone.ParentIndex];
}
else
{
bone = null;
}
}
return(result);
}
private VpdPose GetVpdPose(Dictionary <string, Polyline <JointChange> > curves, PmdModel pmdModel, float time)
{
var vpdPose = new VpdPose();
foreach (var item in curves)
{
var boneName = item.Key;
var curve = item.Value;
var jointChange = curve.Evaluate(time);
vpdPose.SetJointChange(boneName, jointChange);
}
return(vpdPose);
}
internal Pose GetIkLessPose(VpdPose vpdPose)
{
var ikPose = vpdPose.GetIkPose(this);
var normalPose = new Pose(ikPose.NormalPose);
var restArmature = GetRestArmature();
var morphed_armature = new Armature(restArmature);
restArmature.Morph(morphed_armature, normalPose);
var ikArmaturesAndOtherInfo = new List <Tuple <int, string, IKArmature, string, string> >();
for (int i = 0; i < ikChains.Count; i++)
{
var ik_chain = ikChains[i];
var v = GetIkChainIkArmature(i);
var ik_armature = v.Item1;
var root_name = v.Item2;
var end_effector_name = v.Item3;
ikArmaturesAndOtherInfo.Add(Tuple.Create(i, bones[ik_chain.IkBoneIndex].Name, ik_armature, root_name, end_effector_name));
}
ikArmaturesAndOtherInfo = TopologicallySortIkArmatures(ikArmaturesAndOtherInfo);
foreach (var data_quadruple in ikArmaturesAndOtherInfo)
{
var ik_chain_index = data_quadruple.Item1;
var ik_bone_name = data_quadruple.Item2;
var ik_armature = data_quadruple.Item3;
var root_name = data_quadruple.Item4;
var end_effector_name = data_quadruple.Item5;
ComputeIkChainIk(normalPose,
restArmature, morphed_armature,
vpdPose,
ik_bone_name, ik_armature,
root_name, end_effector_name);
}
return(normalPose);
}
private object GetMorphedWorldOrientation(int p, VpdPose vpd_pose)
{
throw new NotImplementedException();
}
private void ComputeIkChainIk(Pose target_pose, Armature rest_armature, Armature morphed_armature, VpdPose vpd_pose, string ik_bone_name, IKArmature ik_armature, string root_name, string end_effector_name)
{
var root_bone = bonesByName[root_name];
var root_position = morphed_armature.GetJointWorldPosition(root_bone.Name);
var root_orientation = morphed_armature.GetJointWorldOrientation(root_bone.Name);
var end_effector_bone = bonesByName[end_effector_name];
var end_effector_position = morphed_armature.GetJointWorldPosition(end_effector_name);
var ik_bone = bonesByName[ik_bone_name];
var goal_position = GetMorphedWorldPosition(ik_bone.Index, vpd_pose);
var goal_orientation = GetMorphedWorldOrientation(ik_bone.Index, vpd_pose);
if (new[] { "leg_ik.R", "leg_ik.L", "tiptoe_ik.R", "tiptoe_ik.L" }.Contains(ik_bone_name))
{
// calculate the rotation need to rotate the vector
// (end_effector - root) to (goal - root)
var root_to_end_effector = end_effector_position - root_position;
var root_to_end_effector_length = root_to_end_effector.Length;
var root_to_goal = goal_position - root_position;
var root_to_goal_length = root_to_goal.Length;
var dot_prod = Vector3D.DotProduct(root_to_end_effector, root_to_goal);
if (root_to_end_effector_length > 0.00001)
{
dot_prod /= root_to_end_effector_length;
}
if (root_to_goal_length > 0.00001)
{
dot_prod /= root_to_goal_length;
}
if (Math.Abs(dot_prod - 1) > 0.0001)
{
var cos_theta = dot_prod;
var theta = Math.Acos(cos_theta);
var axis = Vector3D.CrossProduct(root_to_end_effector, root_to_goal);
axis.Normalize();
var rotation = new Quaternion(axis, theta * 180 / Math.PI);
root_orientation = rotation * root_orientation;
}
}
var chain_rest_armature = ik_armature.GetRestArmature();
chain_rest_armature.SetJointParameter(root_name,
(Vector3D)root_position,
root_orientation);
// create a pose with joint changes
// derived from the VPD pose.
var chain_normal_pose = new Pose();
foreach (var ik_joint in ik_armature.GetIkJoints())
{
var ik_joint_name = ik_joint.Name;
chain_normal_pose.SetJointChange(
ik_joint_name,
target_pose.GetJointChange(ik_joint_name));
}
chain_normal_pose.SetJointChange(root_name, new JointChange());
var chain_ik_pose = new IKPose();
chain_ik_pose.SetNormalPose(chain_normal_pose);
chain_ik_pose.EndEffectorGoals.SetEndEffectorPosition(end_effector_name, goal_position);
var result_pose = new Pose();
ik_armature.SolveForPose(result_pose, chain_ik_pose, 30, 0.0001);
foreach (var ik_joint in ik_armature.GetIkJoints())
{
var joint_change = result_pose.GetJointChange(ik_joint.Name);
target_pose.SetJointChange(ik_joint.Name, joint_change);
}
if (new[] { "leg_ik.R", "leg_ik.L", "tiptoe_ik.R", "tiptoe_ik.L" }.Contains(ik_bone_name))
{
// Fix the root joint change so that it is given
// with respect to its parent.
var root_parent_orientation = morphed_armature.GetJointWorldOrientation(bones[bonesByName[root_name].ParentIndex].Name);
root_parent_orientation.Invert();
var fixed_root_orientation = root_parent_orientation * root_orientation * result_pose.GetJointChange(root_name).Orientation;
target_pose.SetJointChange(root_name,
new JointChange(new Vector3D(0, 0, 0), fixed_root_orientation));
}
rest_armature.Morph(morphed_armature, target_pose);
}
