public static void AddScene(IKEffector e, Color color, bool modifiable, float size)
{
if (!modifiable) return;
// Draw effectors
bool rotate = e.isEndEffector;
float weight = rotate? Mathf.Max(e.positionWeight, e.rotationWeight): e.positionWeight;
if (e.bone != null && weight > 0) {
if (Application.isPlaying) {
Handles.color = new Color(color.r, color.g, color.b, weight);
Handles.DrawLine(e.position, e.bone.position);
Handles.SphereCap(0, e.bone.position, Quaternion.identity, size * 0.5f);
// Manipulating position and rotation
switch(Tools.current) {
case Tool.Move:
e.position = Handles.PositionHandle(e.position, Quaternion.identity);
break;
case Tool.Rotate:
if (rotate) e.rotation = Handles.RotationHandle(e.rotation, e.position);
break;
}
if (rotate) Handles.CubeCap(0, e.position, e.rotation, size);
else Handles.SphereCap(0, e.position, Quaternion.identity, size);
}
}
}
// Initiate this, get the default values
public void Initiate(InteractionSystem interactionSystem) {
this.interactionSystem = interactionSystem;
// Find the effector if we haven't already
if (effector == null) {
effector = interactionSystem.ik.solver.GetEffector(effectorType);
poser = effector.bone.GetComponent<Poser>();
}
StoreDefaults();
}
public override void Execute()
{
Body = Solver.bodyEffector;
LeftFoot = Solver.leftFootEffector;
LeftHand = Solver.leftHandEffector;
LeftShoulder = Solver.leftShoulderEffector;
LeftThigh = Solver.leftThighEffector;
RightFoot = Solver.rightFootEffector;
RightHand = Solver.rightHandEffector;
RightShoulder = Solver.rightShoulderEffector;
RightThigh = Solver.rightThighEffector;
}
// Initiate this, get the default values
public void Initiate(InteractionSystem interactionSystem, IKSolverFullBodyBiped solver) {
this.interactionSystem = interactionSystem;
// Find the effector if we haven't already
if (effector == null) {
effector = solver.GetEffector(effectorType);
poser = effector.bone.GetComponent<Poser>();
}
defaultPull = solver.GetChain(effectorType).pull;
defaultReach = solver.GetChain(effectorType).reach;
defaultPush = solver.GetChain(effectorType).push;
defaultPushParent = solver.GetChain(effectorType).pushParent;
}
//
private void Awake() {
FBIK = GetComponent<FullBodyBipedIK>();
//
Body = FBIK.references.spine[BodySpineID];
BIK = FBIK.solver.bodyEffector;
LFIK = FBIK.solver.leftFootEffector;
RFIK = FBIK.solver.rightFootEffector;
//
LeftFoot = FBIK.references.leftFoot;
RightFoot = FBIK.references.rightFoot;
//
LeftFootContact = new RaycastHit(); RightFootContact = new RaycastHit();
LeftToeContact = new RaycastHit(); RightToeContact = new RaycastHit();
LegLength = Vector3.Distance(LeftFoot.position,Body.position);
}
// Apply the limit to the effector
public void Apply(IKEffector e, Quaternion rootRotation) {
Vector3 offset = Quaternion.Inverse(rootRotation) * e.positionOffset;
if (spring <= 0f) {
// Hard limits
if (x) offset.x = Mathf.Clamp(offset.x, minX, maxX);
if (y) offset.y = Mathf.Clamp(offset.y, minY, maxY);
if (z) offset.z = Mathf.Clamp(offset.z, minZ, maxZ);
} else {
// Soft limits
if (x) offset.x = SpringAxis(offset.x, minX, maxX);
if (y) offset.y = SpringAxis(offset.y, minY, maxY);
if (z) offset.z = SpringAxis(offset.z, minZ, maxZ);
}
// Apply to the effector
e.positionOffset = rootRotation * offset;
}
private void HandsOnProp(IKEffector mainHand, IKEffector otherHand)
{
// Get the animated direction from the main hand to the other hand
Vector3 toOtherHand = otherHand.bone.position - mainHand.bone.position;
// Get the hand direction relative to the main hand's rotation
Vector3 otherHandRelativeDirection = Quaternion.Inverse(mainHand.bone.rotation) * toOtherHand;
// Get the center point of two hands
Vector3 handsCenter = mainHand.bone.position + (toOtherHand * 0.5f);
// Get the other hand's rotation relative to the main hand's rotation
Quaternion otherHandRelativeRotation = Quaternion.Inverse(mainHand.bone.rotation) * otherHand.bone.rotation;
// Get the direction from the main hand to the other hand that icludes effector position offsets
Vector3 toOtherHandWithOffset = (otherHand.bone.position + otherHand.positionOffset) - (mainHand.bone.position + mainHand.positionOffset);
// Get the center point of two hands that includes effector position offsets
Vector3 handsCenterWithOffset = (mainHand.bone.position + mainHand.positionOffset) + (toOtherHand * 0.5f);
// Main hand position
mainHand.position = (mainHand.bone.position + mainHand.positionOffset) + (handsCenterWithOffset - handsCenter);
mainHand.positionWeight = 1f;
// Main hand rotation
Quaternion rotationOffset = Quaternion.FromToRotation(toOtherHand, toOtherHandWithOffset);
mainHand.rotation = rotationOffset * mainHand.bone.rotation;
mainHand.rotationWeight = 1f;
// Other hand position
otherHand.position = mainHand.position + mainHand.rotation * otherHandRelativeDirection;
otherHand.positionWeight = 1f;
// Other hand rotation
otherHand.rotation = mainHand.rotation * otherHandRelativeRotation;
otherHand.rotationWeight = 1f;
}
// Interpolate the solver position of the effector
private void LerpSolverPosition(IKEffector effector, Vector3 position, float weight)
{
effector.GetNode(ik.solver).solverPosition = Vector3.Lerp(effector.GetNode(ik.solver).solverPosition, position, weight);
}
// Set the effector positionOffset for the foot
private void SetLegIK(IKEffector effector, Grounding.Leg leg) {
effector.positionOffset += (leg.IKPosition - effector.bone.position) * weight;
effector.bone.rotation = Quaternion.Slerp(Quaternion.identity, leg.rotationOffset, weight) * effector.bone.rotation;
}
// Rotate an effector from the root of the character
private void RotateEffector(IKEffector effector, Quaternion rotation, float mlp) {
Vector3 d1 = effector.bone.position - transform.position;
Vector3 d2 = rotation * d1;
Vector3 offset = d2 - d1;
effector.positionOffset += offset * mlp;
}
/*
* Get pull offset of a hand
* */
private Vector3 GetHandBodyPull(IKEffector effector, FBIKChain arm, Vector3 offset) {
// Get the vector from shoulder to hand effector
Vector3 direction = effector.position - (arm.nodes[0].transform.position + offset);
float armLength = arm.nodes[0].length + arm.nodes[1].length;
// Find delta of effector distance and arm length
float dirMag = direction.magnitude;
if (dirMag < armLength) return Vector3.zero;
float x = dirMag - armLength;
return (direction / dirMag) * x;
}
/// <summary>
/// Sets up the solver to BipedReferences and reinitiates (if in runtime).
/// </summary>
/// <param name="references">Biped references.</param>
/// <param name="rootNode">Root node (optional). if null, will try to detect the root node bone automatically. </param>
public void SetToReferences(BipedReferences references, Transform rootNode = null) {
root = references.root;
if (rootNode == null) rootNode = DetectRootNodeBone(references);
this.rootNode = rootNode;
// Root Node
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 };
// Left Arm
chain[1].SetNodes(references.leftUpperArm, references.leftForearm, references.leftHand);
// Right Arm
chain[2].SetNodes(references.rightUpperArm, references.rightForearm, references.rightHand);
// Left Leg
chain[3].SetNodes(references.leftThigh, references.leftCalf, references.leftFoot);
// Right Leg
chain[4].SetNodes(references.rightThigh, references.rightCalf, references.rightFoot);
// Effectors
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;
// Child Constraints
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)
};
// IKMappingSpine
Transform[] spineBones = new Transform[references.spine.Length + 1];
spineBones[0] = references.pelvis;
for (int i = 0; i < references.spine.Length; i++) {
spineBones[i + 1] = references.spine[i];
}
if (spineMapping == null) {
spineMapping = new IKMappingSpine();
spineMapping.iterations = 3;
}
spineMapping.SetBones(spineBones, references.leftUpperArm, references.rightUpperArm, references.leftThigh, references.rightThigh);
// IKMappingBone
int boneMappingsCount = references.head != null? 1: 0;
if (boneMappings.Length != boneMappingsCount) {
boneMappings = new IKMappingBone[boneMappingsCount];
for (int i = 0; i < boneMappings.Length; i++) {
boneMappings[i] = new IKMappingBone();
}
if (boneMappingsCount == 1) boneMappings[0].maintainRotationWeight = 0f;
}
if (boneMappings.Length > 0) boneMappings[0].bone = references.head;
// IKMappingLimb
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 void Point(Transform player, Transform target, bool useRightHand)
{
if(!isActive()) {
this.target = target;
if (useRightHand) this.effector = ikSecondary.solver.rightHandEffector;
else this.effector = ikSecondary.solver.leftHandEffector;
this.effector.position = calculateHandPosition(player, target, useRightHand);
state = PointingState.RAISING_ARM;
ikSecondary.solver.spineStiffness = 10f;
}
}
//
private void GetFootContact(IKEffector IK, Transform Bone, Transform ToeTip, Vector3 BoneSpeed, RaycastHit HIT, RaycastHit THIT) {
Ray RAY1 = new Ray(new Vector3(IK.position.x,BIK.position.y,IK.position.z),new Vector3(0,-1,0));
Ray RAY2; if (ToeTip) {RAY2 = new Ray(new Vector3(ToeTip.position.x,BIK.position.y,ToeTip.position.z),new Vector3(0,-1,0));} else {RAY2 = RAY1;}
//
if (Physics.Raycast(RAY1,out HIT,(LegLength*RayReach),FeetMask.value)) {
IK.positionWeight = 1f; IK.rotationWeight = 1f;
IK.position = Vector3.Lerp(IK.position,new Vector3(Bone.position.x,HIT.point.y+HeelOffset,Bone.position.z),Time.time);
IK.rotation = Quaternion.FromToRotation(Vector3.up,HIT.normal)*IK.bone.rotation;
Debug.DrawLine(RAY1.origin,BIK.position,Color.cyan); Debug.DrawLine(RAY1.origin,IK.position,Color.green); Debug.DrawLine(IK.position,HIT.point,Color.red);
}
//
if (ToeTip && Physics.Raycast(RAY2,out THIT,(LegLength*RayReach),FeetMask.value) && THIT.point.y > HIT.point.y) {
IK.positionWeight = 1f; IK.rotationWeight = 1f;
IK.position = Vector3.Lerp(IK.position,new Vector3(Bone.position.x,THIT.point.y+HeelOffset,Bone.position.z),Time.time);
IK.rotation = Quaternion.FromToRotation(Vector3.up,THIT.normal)*IK.bone.rotation;
Debug.DrawLine(RAY2.origin,BIK.position,Color.cyan); Debug.DrawLine(RAY2.origin,ToeTip.position,Color.green); Debug.DrawLine(ToeTip.position,THIT.point,Color.red);
}
//
if (IK.position.y < THIT.point.y) {IK.position = new Vector3(Bone.position.x,THIT.point.y+HeelOffset,Bone.position.z);}
}
// Initiate this, get the default values
public void Initiate(IKSolverFullBodyBiped solver)
{
// Find the effector if we haven't already
if (effector == null) {
effector = solver.GetEffector(effectorType);
poser = effector.bone.GetComponent<Poser>();
}
defaultPull = solver.GetChain(effectorType).pull;
defaultReach = solver.GetChain(effectorType).reach;
}
