486 lines
15 KiB
C#
486 lines
15 KiB
C#
using System;
|
|
using UnityEngine;
|
|
|
|
namespace RootMotion.FinalIK
|
|
{
|
|
[Serializable]
|
|
public class IKSolverFullBodyBiped : IKSolverFullBody
|
|
{
|
|
public Transform rootNode;
|
|
|
|
[Range(0f, 1f)]
|
|
public float spineStiffness = 0.5f;
|
|
|
|
[Range(-1f, 1f)]
|
|
public float pullBodyVertical = 0.5f;
|
|
|
|
[Range(-1f, 1f)]
|
|
public float pullBodyHorizontal;
|
|
|
|
private Vector3 offset;
|
|
|
|
public IKEffector bodyEffector => GetEffector(FullBodyBipedEffector.Body);
|
|
|
|
public IKEffector leftShoulderEffector => GetEffector(FullBodyBipedEffector.LeftShoulder);
|
|
|
|
public IKEffector rightShoulderEffector => GetEffector(FullBodyBipedEffector.RightShoulder);
|
|
|
|
public IKEffector leftThighEffector => GetEffector(FullBodyBipedEffector.LeftThigh);
|
|
|
|
public IKEffector rightThighEffector => GetEffector(FullBodyBipedEffector.RightThigh);
|
|
|
|
public IKEffector leftHandEffector => GetEffector(FullBodyBipedEffector.LeftHand);
|
|
|
|
public IKEffector rightHandEffector => GetEffector(FullBodyBipedEffector.RightHand);
|
|
|
|
public IKEffector leftFootEffector => GetEffector(FullBodyBipedEffector.LeftFoot);
|
|
|
|
public IKEffector rightFootEffector => GetEffector(FullBodyBipedEffector.RightFoot);
|
|
|
|
public FBIKChain leftArmChain => chain[1];
|
|
|
|
public FBIKChain rightArmChain => chain[2];
|
|
|
|
public FBIKChain leftLegChain => chain[3];
|
|
|
|
public FBIKChain rightLegChain => chain[4];
|
|
|
|
public IKMappingLimb leftArmMapping => limbMappings[0];
|
|
|
|
public IKMappingLimb rightArmMapping => limbMappings[1];
|
|
|
|
public IKMappingLimb leftLegMapping => limbMappings[2];
|
|
|
|
public IKMappingLimb rightLegMapping => limbMappings[3];
|
|
|
|
public IKMappingBone headMapping => boneMappings[0];
|
|
|
|
public Vector3 pullBodyOffset { get; private set; }
|
|
|
|
public void SetChainWeights(FullBodyBipedChain c, float pull, float reach = 0f)
|
|
{
|
|
GetChain(c).pull = pull;
|
|
GetChain(c).reach = reach;
|
|
}
|
|
|
|
public void SetEffectorWeights(FullBodyBipedEffector effector, float positionWeight, float rotationWeight)
|
|
{
|
|
GetEffector(effector).positionWeight = Mathf.Clamp(positionWeight, 0f, 1f);
|
|
GetEffector(effector).rotationWeight = Mathf.Clamp(rotationWeight, 0f, 1f);
|
|
}
|
|
|
|
public FBIKChain GetChain(FullBodyBipedChain c)
|
|
{
|
|
return c switch
|
|
{
|
|
FullBodyBipedChain.LeftArm => chain[1],
|
|
FullBodyBipedChain.RightArm => chain[2],
|
|
FullBodyBipedChain.LeftLeg => chain[3],
|
|
FullBodyBipedChain.RightLeg => chain[4],
|
|
_ => null,
|
|
};
|
|
}
|
|
|
|
public FBIKChain GetChain(FullBodyBipedEffector effector)
|
|
{
|
|
return effector switch
|
|
{
|
|
FullBodyBipedEffector.Body => chain[0],
|
|
FullBodyBipedEffector.LeftShoulder => chain[1],
|
|
FullBodyBipedEffector.RightShoulder => chain[2],
|
|
FullBodyBipedEffector.LeftThigh => chain[3],
|
|
FullBodyBipedEffector.RightThigh => chain[4],
|
|
FullBodyBipedEffector.LeftHand => chain[1],
|
|
FullBodyBipedEffector.RightHand => chain[2],
|
|
FullBodyBipedEffector.LeftFoot => chain[3],
|
|
FullBodyBipedEffector.RightFoot => chain[4],
|
|
_ => null,
|
|
};
|
|
}
|
|
|
|
public IKEffector GetEffector(FullBodyBipedEffector effector)
|
|
{
|
|
return effector switch
|
|
{
|
|
FullBodyBipedEffector.Body => effectors[0],
|
|
FullBodyBipedEffector.LeftShoulder => effectors[1],
|
|
FullBodyBipedEffector.RightShoulder => effectors[2],
|
|
FullBodyBipedEffector.LeftThigh => effectors[3],
|
|
FullBodyBipedEffector.RightThigh => effectors[4],
|
|
FullBodyBipedEffector.LeftHand => effectors[5],
|
|
FullBodyBipedEffector.RightHand => effectors[6],
|
|
FullBodyBipedEffector.LeftFoot => effectors[7],
|
|
FullBodyBipedEffector.RightFoot => effectors[8],
|
|
_ => null,
|
|
};
|
|
}
|
|
|
|
public IKEffector GetEndEffector(FullBodyBipedChain c)
|
|
{
|
|
return c switch
|
|
{
|
|
FullBodyBipedChain.LeftArm => effectors[5],
|
|
FullBodyBipedChain.RightArm => effectors[6],
|
|
FullBodyBipedChain.LeftLeg => effectors[7],
|
|
FullBodyBipedChain.RightLeg => effectors[8],
|
|
_ => null,
|
|
};
|
|
}
|
|
|
|
public IKMappingLimb GetLimbMapping(FullBodyBipedChain chain)
|
|
{
|
|
return chain switch
|
|
{
|
|
FullBodyBipedChain.LeftArm => limbMappings[0],
|
|
FullBodyBipedChain.RightArm => limbMappings[1],
|
|
FullBodyBipedChain.LeftLeg => limbMappings[2],
|
|
FullBodyBipedChain.RightLeg => limbMappings[3],
|
|
_ => null,
|
|
};
|
|
}
|
|
|
|
public IKMappingLimb GetLimbMapping(FullBodyBipedEffector effector)
|
|
{
|
|
return effector switch
|
|
{
|
|
FullBodyBipedEffector.LeftShoulder => limbMappings[0],
|
|
FullBodyBipedEffector.RightShoulder => limbMappings[1],
|
|
FullBodyBipedEffector.LeftThigh => limbMappings[2],
|
|
FullBodyBipedEffector.RightThigh => limbMappings[3],
|
|
FullBodyBipedEffector.LeftHand => limbMappings[0],
|
|
FullBodyBipedEffector.RightHand => limbMappings[1],
|
|
FullBodyBipedEffector.LeftFoot => limbMappings[2],
|
|
FullBodyBipedEffector.RightFoot => limbMappings[3],
|
|
_ => null,
|
|
};
|
|
}
|
|
|
|
public IKMappingSpine GetSpineMapping()
|
|
{
|
|
return spineMapping;
|
|
}
|
|
|
|
public IKMappingBone GetHeadMapping()
|
|
{
|
|
return boneMappings[0];
|
|
}
|
|
|
|
public IKConstraintBend GetBendConstraint(FullBodyBipedChain limb)
|
|
{
|
|
return limb switch
|
|
{
|
|
FullBodyBipedChain.LeftArm => chain[1].bendConstraint,
|
|
FullBodyBipedChain.RightArm => chain[2].bendConstraint,
|
|
FullBodyBipedChain.LeftLeg => chain[3].bendConstraint,
|
|
FullBodyBipedChain.RightLeg => chain[4].bendConstraint,
|
|
_ => null,
|
|
};
|
|
}
|
|
|
|
public override bool IsValid(ref string message)
|
|
{
|
|
if (!base.IsValid(ref message))
|
|
{
|
|
return false;
|
|
}
|
|
if (rootNode == null)
|
|
{
|
|
message = "Root Node bone is null. FBBIK will not initiate.";
|
|
return false;
|
|
}
|
|
if (chain.Length != 5 || chain[0].nodes.Length != 1 || chain[1].nodes.Length != 3 || chain[2].nodes.Length != 3 || chain[3].nodes.Length != 3 || chain[4].nodes.Length != 3 || effectors.Length != 9 || limbMappings.Length != 4)
|
|
{
|
|
message = "Invalid FBBIK setup. Please right-click on the component header and select 'Reinitiate'.";
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
public void SetToReferences(BipedReferences references, Transform rootNode = null)
|
|
{
|
|
root = references.root;
|
|
if (rootNode == null)
|
|
{
|
|
rootNode = DetectRootNodeBone(references);
|
|
}
|
|
this.rootNode = rootNode;
|
|
if (chain == null || chain.Length != 5)
|
|
{
|
|
chain = new FBIKChain[5];
|
|
}
|
|
for (int i = 0; i < chain.Length; i++)
|
|
{
|
|
if (chain[i] == null)
|
|
{
|
|
chain[i] = new FBIKChain();
|
|
}
|
|
}
|
|
chain[0].pin = 0f;
|
|
chain[0].SetNodes(rootNode);
|
|
chain[0].children = new int[4] { 1, 2, 3, 4 };
|
|
chain[1].SetNodes(references.leftUpperArm, references.leftForearm, references.leftHand);
|
|
chain[2].SetNodes(references.rightUpperArm, references.rightForearm, references.rightHand);
|
|
chain[3].SetNodes(references.leftThigh, references.leftCalf, references.leftFoot);
|
|
chain[4].SetNodes(references.rightThigh, references.rightCalf, references.rightFoot);
|
|
if (effectors.Length != 9)
|
|
{
|
|
effectors = new IKEffector[9]
|
|
{
|
|
new IKEffector(),
|
|
new IKEffector(),
|
|
new IKEffector(),
|
|
new IKEffector(),
|
|
new IKEffector(),
|
|
new IKEffector(),
|
|
new IKEffector(),
|
|
new IKEffector(),
|
|
new IKEffector()
|
|
};
|
|
}
|
|
effectors[0].bone = rootNode;
|
|
effectors[0].childBones = new Transform[2] { references.leftThigh, references.rightThigh };
|
|
effectors[1].bone = references.leftUpperArm;
|
|
effectors[2].bone = references.rightUpperArm;
|
|
effectors[3].bone = references.leftThigh;
|
|
effectors[4].bone = references.rightThigh;
|
|
effectors[5].bone = references.leftHand;
|
|
effectors[6].bone = references.rightHand;
|
|
effectors[7].bone = references.leftFoot;
|
|
effectors[8].bone = references.rightFoot;
|
|
effectors[5].planeBone1 = references.leftUpperArm;
|
|
effectors[5].planeBone2 = references.rightUpperArm;
|
|
effectors[5].planeBone3 = rootNode;
|
|
effectors[6].planeBone1 = references.rightUpperArm;
|
|
effectors[6].planeBone2 = references.leftUpperArm;
|
|
effectors[6].planeBone3 = rootNode;
|
|
effectors[7].planeBone1 = references.leftThigh;
|
|
effectors[7].planeBone2 = references.rightThigh;
|
|
effectors[7].planeBone3 = rootNode;
|
|
effectors[8].planeBone1 = references.rightThigh;
|
|
effectors[8].planeBone2 = references.leftThigh;
|
|
effectors[8].planeBone3 = rootNode;
|
|
chain[0].childConstraints = new FBIKChain.ChildConstraint[4]
|
|
{
|
|
new FBIKChain.ChildConstraint(references.leftUpperArm, references.rightThigh, 0f, 1f),
|
|
new FBIKChain.ChildConstraint(references.rightUpperArm, references.leftThigh, 0f, 1f),
|
|
new FBIKChain.ChildConstraint(references.leftUpperArm, references.rightUpperArm),
|
|
new FBIKChain.ChildConstraint(references.leftThigh, references.rightThigh)
|
|
};
|
|
Transform[] array = new Transform[references.spine.Length + 1];
|
|
array[0] = references.pelvis;
|
|
for (int j = 0; j < references.spine.Length; j++)
|
|
{
|
|
array[j + 1] = references.spine[j];
|
|
}
|
|
if (spineMapping == null)
|
|
{
|
|
spineMapping = new IKMappingSpine();
|
|
spineMapping.iterations = 3;
|
|
}
|
|
spineMapping.SetBones(array, references.leftUpperArm, references.rightUpperArm, references.leftThigh, references.rightThigh);
|
|
int num = ((references.head != null) ? 1 : 0);
|
|
if (boneMappings.Length != num)
|
|
{
|
|
boneMappings = new IKMappingBone[num];
|
|
for (int k = 0; k < boneMappings.Length; k++)
|
|
{
|
|
boneMappings[k] = new IKMappingBone();
|
|
}
|
|
if (num == 1)
|
|
{
|
|
boneMappings[0].maintainRotationWeight = 0f;
|
|
}
|
|
}
|
|
if (boneMappings.Length != 0)
|
|
{
|
|
boneMappings[0].bone = references.head;
|
|
}
|
|
if (limbMappings.Length != 4)
|
|
{
|
|
limbMappings = new IKMappingLimb[4]
|
|
{
|
|
new IKMappingLimb(),
|
|
new IKMappingLimb(),
|
|
new IKMappingLimb(),
|
|
new IKMappingLimb()
|
|
};
|
|
limbMappings[2].maintainRotationWeight = 1f;
|
|
limbMappings[3].maintainRotationWeight = 1f;
|
|
}
|
|
limbMappings[0].SetBones(references.leftUpperArm, references.leftForearm, references.leftHand, GetLeftClavicle(references));
|
|
limbMappings[1].SetBones(references.rightUpperArm, references.rightForearm, references.rightHand, GetRightClavicle(references));
|
|
limbMappings[2].SetBones(references.leftThigh, references.leftCalf, references.leftFoot);
|
|
limbMappings[3].SetBones(references.rightThigh, references.rightCalf, references.rightFoot);
|
|
if (Application.isPlaying)
|
|
{
|
|
Initiate(references.root);
|
|
}
|
|
}
|
|
|
|
public static Transform DetectRootNodeBone(BipedReferences references)
|
|
{
|
|
if (!references.isFilled)
|
|
{
|
|
return null;
|
|
}
|
|
if (references.spine.Length < 1)
|
|
{
|
|
return null;
|
|
}
|
|
int num = references.spine.Length;
|
|
if (num == 1)
|
|
{
|
|
return references.spine[0];
|
|
}
|
|
Vector3 vector = Vector3.Lerp(references.leftThigh.position, references.rightThigh.position, 0.5f);
|
|
Vector3 onNormal = Vector3.Lerp(references.leftUpperArm.position, references.rightUpperArm.position, 0.5f) - vector;
|
|
float magnitude = onNormal.magnitude;
|
|
if (references.spine.Length < 2)
|
|
{
|
|
return references.spine[0];
|
|
}
|
|
int num2 = 0;
|
|
for (int i = 1; i < num; i++)
|
|
{
|
|
Vector3 vector2 = Vector3.Project(references.spine[i].position - vector, onNormal);
|
|
if (Vector3.Dot(vector2.normalized, onNormal.normalized) > 0f && vector2.magnitude / magnitude < 0.5f)
|
|
{
|
|
num2 = i;
|
|
}
|
|
}
|
|
return references.spine[num2];
|
|
}
|
|
|
|
public void SetLimbOrientations(BipedLimbOrientations o)
|
|
{
|
|
SetLimbOrientation(FullBodyBipedChain.LeftArm, o.leftArm);
|
|
SetLimbOrientation(FullBodyBipedChain.RightArm, o.rightArm);
|
|
SetLimbOrientation(FullBodyBipedChain.LeftLeg, o.leftLeg);
|
|
SetLimbOrientation(FullBodyBipedChain.RightLeg, o.rightLeg);
|
|
}
|
|
|
|
private void SetLimbOrientation(FullBodyBipedChain chain, BipedLimbOrientations.LimbOrientation limbOrientation)
|
|
{
|
|
if (chain == FullBodyBipedChain.LeftArm || chain == FullBodyBipedChain.RightArm)
|
|
{
|
|
GetBendConstraint(chain).SetLimbOrientation(-limbOrientation.upperBoneForwardAxis, -limbOrientation.lowerBoneForwardAxis, -limbOrientation.lastBoneLeftAxis);
|
|
GetLimbMapping(chain).SetLimbOrientation(-limbOrientation.upperBoneForwardAxis, -limbOrientation.lowerBoneForwardAxis);
|
|
}
|
|
else
|
|
{
|
|
GetBendConstraint(chain).SetLimbOrientation(limbOrientation.upperBoneForwardAxis, limbOrientation.lowerBoneForwardAxis, limbOrientation.lastBoneLeftAxis);
|
|
GetLimbMapping(chain).SetLimbOrientation(limbOrientation.upperBoneForwardAxis, limbOrientation.lowerBoneForwardAxis);
|
|
}
|
|
}
|
|
|
|
private static Transform GetLeftClavicle(BipedReferences references)
|
|
{
|
|
if (references.leftUpperArm == null)
|
|
{
|
|
return null;
|
|
}
|
|
if (!Contains(references.spine, references.leftUpperArm.parent))
|
|
{
|
|
return references.leftUpperArm.parent;
|
|
}
|
|
return null;
|
|
}
|
|
|
|
private static Transform GetRightClavicle(BipedReferences references)
|
|
{
|
|
if (references.rightUpperArm == null)
|
|
{
|
|
return null;
|
|
}
|
|
if (!Contains(references.spine, references.rightUpperArm.parent))
|
|
{
|
|
return references.rightUpperArm.parent;
|
|
}
|
|
return null;
|
|
}
|
|
|
|
private static bool Contains(Transform[] array, Transform transform)
|
|
{
|
|
for (int i = 0; i < array.Length; i++)
|
|
{
|
|
if (array[i] == transform)
|
|
{
|
|
return true;
|
|
}
|
|
}
|
|
return false;
|
|
}
|
|
|
|
protected override void ReadPose()
|
|
{
|
|
for (int i = 0; i < effectors.Length; i++)
|
|
{
|
|
effectors[i].SetToTarget();
|
|
}
|
|
PullBody();
|
|
float pushElasticity = Mathf.Clamp(1f - spineStiffness, 0f, 1f);
|
|
chain[0].childConstraints[0].pushElasticity = pushElasticity;
|
|
chain[0].childConstraints[1].pushElasticity = pushElasticity;
|
|
base.ReadPose();
|
|
}
|
|
|
|
private void PullBody()
|
|
{
|
|
if (iterations >= 1 && (pullBodyVertical != 0f || pullBodyHorizontal != 0f))
|
|
{
|
|
Vector3 bodyOffset = GetBodyOffset();
|
|
pullBodyOffset = V3Tools.ExtractVertical(bodyOffset, root.up, pullBodyVertical) + V3Tools.ExtractHorizontal(bodyOffset, root.up, pullBodyHorizontal);
|
|
bodyEffector.positionOffset += pullBodyOffset;
|
|
}
|
|
}
|
|
|
|
private Vector3 GetBodyOffset()
|
|
{
|
|
Vector3 vector = Vector3.zero + GetHandBodyPull(leftHandEffector, leftArmChain, Vector3.zero) * Mathf.Clamp(leftHandEffector.positionWeight, 0f, 1f);
|
|
return vector + GetHandBodyPull(rightHandEffector, rightArmChain, vector) * Mathf.Clamp(rightHandEffector.positionWeight, 0f, 1f);
|
|
}
|
|
|
|
private Vector3 GetHandBodyPull(IKEffector effector, FBIKChain arm, Vector3 offset)
|
|
{
|
|
Vector3 vector = effector.position - (arm.nodes[0].transform.position + offset);
|
|
float num = arm.nodes[0].length + arm.nodes[1].length;
|
|
float magnitude = vector.magnitude;
|
|
if (magnitude < num)
|
|
{
|
|
return Vector3.zero;
|
|
}
|
|
float num2 = magnitude - num;
|
|
return vector / magnitude * num2;
|
|
}
|
|
|
|
protected override void ApplyBendConstraints()
|
|
{
|
|
if (iterations > 0)
|
|
{
|
|
chain[1].bendConstraint.rotationOffset = leftHandEffector.planeRotationOffset;
|
|
chain[2].bendConstraint.rotationOffset = rightHandEffector.planeRotationOffset;
|
|
chain[3].bendConstraint.rotationOffset = leftFootEffector.planeRotationOffset;
|
|
chain[4].bendConstraint.rotationOffset = rightFootEffector.planeRotationOffset;
|
|
}
|
|
else
|
|
{
|
|
offset = Vector3.Lerp(effectors[0].positionOffset, effectors[0].position - (effectors[0].bone.position + effectors[0].positionOffset), effectors[0].positionWeight);
|
|
for (int i = 0; i < 5; i++)
|
|
{
|
|
effectors[i].GetNode(this).solverPosition += offset;
|
|
}
|
|
}
|
|
base.ApplyBendConstraints();
|
|
}
|
|
|
|
protected override void WritePose()
|
|
{
|
|
if (iterations == 0)
|
|
{
|
|
spineMapping.spineBones[0].position += offset;
|
|
}
|
|
base.WritePose();
|
|
}
|
|
}
|
|
}
|