internal IKPose GetIkPose(PmdModel pmdModel)
{
var ikPose = new IKPose();
var normalPose = ikPose.NormalPose;
var endEffectorGoals = ikPose.EndEffectorGoals;
var assignableBoneTypes = new[] { Bone.BONE_ROTATE,
Bone.BONE_ROTATE_TRANSLATE,
Bone.BONE_IK_ROTATION_INFLUENCED,
Bone.BONE_IK_TARGET };
JointChange jointChange;
for (int boneIndex = 0; boneIndex < pmdModel.Bones.Count; boneIndex++)
{
var bone = pmdModel.Bones.ElementAt(boneIndex);
if (assignableBoneTypes.Contains(bone.BoneType))
{
try
{
jointChange = GetRightHandedJointChange(bone.Name);
}
catch (KeyNotFoundException)
{
continue;
}
normalPose.SetJointChange(bone.Name, jointChange);
}
else if (bone.BoneType == Bone.BONE_ROTATION_INFLUENCED)
{
var source_bone = pmdModel.Bones.ElementAt(bone.IkBoneIndex);
try
{
jointChange = GetRightHandedJointChange(source_bone.Name);
}
catch (KeyNotFoundException)
{
continue;
}
normalPose.SetJointChange(bone.Name, jointChange);
}
else if (bone.BoneType == Bone.BONE_IK)
{
var ikChain = pmdModel.GetIkChainByIkBoneIndex(boneIndex);
var endEffectorBone = pmdModel.Bones.ElementAt(ikChain.EndEffectorIndex);
var newPosition = pmdModel.GetMorphedWorldPosition(boneIndex, this);
endEffectorGoals.SetEndEffectorPosition(endEffectorBone.Name, newPosition);
}
}
return(ikPose);
}
void SetIKLimbTargets(IKPose pose)
{
currentPose = pose;
if (pose == controllerPose)
{
}
leftHand.target = pose.leftHand.target;
leftHand.elbowTarget = pose.leftHand.elbow;
/*
* if (pose.leftHand.fingers != null) {
* leftThumb.IsEnabled = true;
* leftIndex.IsEnabled = true;
* leftMiddle.IsEnabled = true;
* leftRing.IsEnabled = true;
* //leftPinky.IsEnabled = true;
*
* leftThumb.target = pose.leftHand.fingers.thumb.target;
* leftThumb.elbowTarget = pose.leftHand.fingers.thumb.elbowTarget;
* leftIndex.target = pose.leftHand.fingers.index.target;
* leftIndex.elbowTarget = pose.leftHand.fingers.index.elbowTarget;
* leftMiddle.target = pose.leftHand.fingers.middle.target;
* leftMiddle.elbowTarget = pose.leftHand.fingers.middle.elbowTarget;
* leftRing.target = pose.leftHand.fingers.ring.target;
* leftRing.elbowTarget = pose.leftHand.fingers.ring.elbowTarget;
* //leftPinky.target = pose.leftHand.fingers.pinky.target;
* //leftPinky.elbowTarget = pose.leftHand.fingers.pinky.elbowTarget;
* }else
* {
* leftThumb.IsEnabled = false;
* leftIndex.IsEnabled = false;
* leftMiddle.IsEnabled = false;
* leftRing.IsEnabled = false;
* leftPinky.IsEnabled = false;
* }
*/
rightHand.target = pose.rightHand.target;
rightHand.elbowTarget = pose.rightHand.elbow;
/*
* if (pose.rightHand.fingers != null) {
* rightThumb.IsEnabled = true;
* rightIndex.IsEnabled = true;
* rightMiddle.IsEnabled = true;
* rightRing.IsEnabled = true;
* rightPinky.IsEnabled = true;
*
* rightThumb.target = pose.rightHand.fingers.thumb.target;
* rightThumb.elbowTarget = pose.rightHand.fingers.thumb.elbowTarget;
* rightIndex.target = pose.rightHand.fingers.index.target;
* rightIndex.elbowTarget = pose.rightHand.fingers.index.elbowTarget;
* rightMiddle.target = pose.rightHand.fingers.middle.target;
* rightMiddle.elbowTarget = pose.rightHand.fingers.middle.elbowTarget;
* rightRing.target = pose.rightHand.fingers.ring.target;
* rightRing.elbowTarget = pose.rightHand.fingers.ring.elbowTarget;
* rightPinky.target = pose.rightHand.fingers.pinky.target;
* rightPinky.elbowTarget = pose.rightHand.fingers.pinky.elbowTarget;
* }else{
* rightThumb.IsEnabled = false;
* rightIndex.IsEnabled = false;
* rightMiddle.IsEnabled = false;
* rightRing.IsEnabled = false;
* rightPinky.IsEnabled = false;
* }
*/
}
void Relaxed()
{
Debug.Log("Relaxing hands.");
currentPose = relaxedPose;
}
void GripController()
{
Debug.Log("Gripping controller.");
currentPose = controllerPose;
}
public PoseAnimation GetIkLessPoseAnimation(PmdModel pmdModel, Func <double, bool> progressHook)
{
var curves = GetBoneCurves();
var ikArmature = pmdModel.GetIkArmature();
var ikPose = new IKPose();
var pose = new Pose();
var iterators = new Dictionary <string, IEnumerator <ControlPoint <JointChange> > >();
foreach (var item in curves)
{
var boneName = item.Key;
var curve = item.Value;
iterators[boneName] = curve.GetControlPointIterator();
}
var frames = new Dictionary <string, ControlPoint <JointChange> >();
foreach (var item in iterators)
{
var boneName = item.Key;
var iterator = item.Value;
if (iterator.MoveNext())
{
frames[boneName] = iterator.Current;
}
}
var ikJointCurves = new Dictionary <string, Polyline <JointChange> >();
foreach (var ikJoint in ikArmature.GetIkJoints())
{
ikJointCurves[ikJoint.Name] = new Polyline_JointChange();
}
while (frames.Count > 0)
{
var earliestTime = 1e20f;
foreach (var item in frames)
{
var boneName = item.Key;
var frame = item.Value;
if (frame.Time < earliestTime)
{
earliestTime = frame.Time;
}
}
var vpd_pose = GetVpdPose(curves, pmdModel, earliestTime);
pose = pmdModel.GetIkLessPose(vpd_pose);
foreach (var item in ikJointCurves)
{
var ikJointName = item.Key;
var curve = item.Value;
curve.SetControlPoint(earliestTime, pose.GetJointChange(ikJointName));
}
var boneNamesWithEarliestTime = new List <string>();
foreach (var item in frames)
{
var boneName = item.Key;
var frame = item.Value;
if (Math.Abs(frame.Time - earliestTime) < 0.0001)
{
boneNamesWithEarliestTime.Add(boneName);
}
}
foreach (var boneName in boneNamesWithEarliestTime)
{
var iterator = iterators[boneName];
if (iterator.MoveNext())
{
frames[boneName] = iterator.Current;
}
else
{
frames.Remove(boneName);
}
}
if (progressHook != null)
{
var continuing = progressHook(earliestTime);
if (!continuing)
{
return(null);
}
}
}
var poseAnimation = new PoseAnimation();
curves = new Dictionary <string, Polyline <JointChange> >();
foreach (var boneFrame in boneFrames)
{
if (!curves.ContainsKey(boneFrame.BoneName))
{
curves[boneFrame.BoneName] = new Polyline_JointChange();
}
var position = new Vector3D(boneFrame.Position.X, boneFrame.Position.Y, -boneFrame.Position.Z);
// var z_flip_matrix = matrix4x4.scale(1, 1, -1);
var orientation = new Quaternion(boneFrame.Orientation.X,
boneFrame.Orientation.Y,
-boneFrame.Orientation.Z,
-boneFrame.Orientation.W);
curves[boneFrame.BoneName].SetControlPoint(boneFrame.FrameNumber,
new JointChange(position, orientation));
}
foreach (var item in curves)
{
var boneName = item.Key;
var curve = item.Value;
if (pmdModel.BonesByName.ContainsKey(boneName))
{
var bone = pmdModel.GetBoneByName(boneName);
if (bone.BoneType != Bone.BONE_IK)
{
poseAnimation.SetJointCurve(boneName, curve);
}
}
}
foreach (var item in ikJointCurves)
{
var boneName = item.Key;
var ikJointCurve = item.Value;
poseAnimation.SetJointCurve(boneName, ikJointCurve);
}
foreach (var bone in pmdModel.Bones)
{
if (bone.BoneType == Bone.BONE_ROTATION_INFLUENCED)
{
var sourceBone = pmdModel.Bones.ElementAt(bone.IkBoneIndex);
var boneCurve = new Polyline_JointChange();
if (curves.ContainsKey(sourceBone.Name))
{
var sourceCurve = curves[sourceBone.Name];
foreach (var controlPoint in sourceCurve.GetControlPoints())
{
var time = controlPoint.Time;
var jc = controlPoint.Value;
boneCurve.SetControlPoint(time, new JointChange(new Vector3D(0, 0, 0), jc.Orientation));
}
}
poseAnimation.SetJointCurve(bone.Name, boneCurve);
}
}
poseAnimation.UpdateRange();
return(poseAnimation);
}
void SetIKLimbTargets(IKPose pose)
{
currentPose = pose;
if (pose == controllerPose) {
}
leftHand.target = pose.leftHand.target;
leftHand.elbowTarget = pose.leftHand.elbow;
/*
if (pose.leftHand.fingers != null) {
leftThumb.IsEnabled = true;
leftIndex.IsEnabled = true;
leftMiddle.IsEnabled = true;
leftRing.IsEnabled = true;
//leftPinky.IsEnabled = true;
leftThumb.target = pose.leftHand.fingers.thumb.target;
leftThumb.elbowTarget = pose.leftHand.fingers.thumb.elbowTarget;
leftIndex.target = pose.leftHand.fingers.index.target;
leftIndex.elbowTarget = pose.leftHand.fingers.index.elbowTarget;
leftMiddle.target = pose.leftHand.fingers.middle.target;
leftMiddle.elbowTarget = pose.leftHand.fingers.middle.elbowTarget;
leftRing.target = pose.leftHand.fingers.ring.target;
leftRing.elbowTarget = pose.leftHand.fingers.ring.elbowTarget;
//leftPinky.target = pose.leftHand.fingers.pinky.target;
//leftPinky.elbowTarget = pose.leftHand.fingers.pinky.elbowTarget;
}else
{
leftThumb.IsEnabled = false;
leftIndex.IsEnabled = false;
leftMiddle.IsEnabled = false;
leftRing.IsEnabled = false;
leftPinky.IsEnabled = false;
}
*/
rightHand.target = pose.rightHand.target;
rightHand.elbowTarget = pose.rightHand.elbow;
/*
if (pose.rightHand.fingers != null) {
rightThumb.IsEnabled = true;
rightIndex.IsEnabled = true;
rightMiddle.IsEnabled = true;
rightRing.IsEnabled = true;
rightPinky.IsEnabled = true;
rightThumb.target = pose.rightHand.fingers.thumb.target;
rightThumb.elbowTarget = pose.rightHand.fingers.thumb.elbowTarget;
rightIndex.target = pose.rightHand.fingers.index.target;
rightIndex.elbowTarget = pose.rightHand.fingers.index.elbowTarget;
rightMiddle.target = pose.rightHand.fingers.middle.target;
rightMiddle.elbowTarget = pose.rightHand.fingers.middle.elbowTarget;
rightRing.target = pose.rightHand.fingers.ring.target;
rightRing.elbowTarget = pose.rightHand.fingers.ring.elbowTarget;
rightPinky.target = pose.rightHand.fingers.pinky.target;
rightPinky.elbowTarget = pose.rightHand.fingers.pinky.elbowTarget;
}else{
rightThumb.IsEnabled = false;
rightIndex.IsEnabled = false;
rightMiddle.IsEnabled = false;
rightRing.IsEnabled = false;
rightPinky.IsEnabled = false;
}
*/
}
void Relaxed()
{
Debug.Log ("Relaxing hands.");
currentPose = relaxedPose;
}
void GripController()
{
Debug.Log ("Gripping controller.");
currentPose = controllerPose;
}
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);
}
