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); }