public void SetupFromHead()
{
feet = new List <BoneJoint>();
List <BoneJoint> bodyJointsToVisit = new List <BoneJoint>();
bodyJointsToVisit.Add(head);
boneCount = 0;
float maxDistFromBodyJoint = 0f;
float maxDistFromHead = 0f;
BoneJoint latestBodyJoint = null;
while (bodyJointsToVisit.Count > 0)
{
BoneJoint currentJoint = bodyJointsToVisit[0];
boneCount++;
maxDistFromHead += currentJoint.distanceFromLastBone;
currentJoint.maxDistanceFromHead = maxDistFromHead;
latestBodyJoint = currentJoint;
maxDistFromBodyJoint = 0f;
if (baseOfNeck == null)
{
if (currentJoint.nextJoints != null && currentJoint.nextJoints.Count >= 2)
{
baseOfNeck = currentJoint;
}
}
else if (currentJoint.nextJoints != null)
{
baseOfTail = currentJoint;
}
if (baseOfTail == null)
{
baseOfTail = currentJoint;
}
endOfTail = currentJoint;
for (int i = 0; i < currentJoint.nextJoints.Count; i++)
{
BoneJoint nextJoint = currentJoint.nextJoints[i];
if (nextJoint.isPartOfLeg())
{
while (nextJoint != null)
{
boneCount++;
maxDistFromBodyJoint += nextJoint.distanceFromLastBone;
nextJoint.maxDistanceFromHead = maxDistFromHead + maxDistFromBodyJoint;
nextJoint.maxDistanceFromBodyJoint = maxDistFromBodyJoint;
nextJoint.closestBodyJoint = latestBodyJoint;
if (nextJoint.nextJoints == null || nextJoint.nextJoints.Count == 0)
{
nextJoint.type = BoneJoint.BoneJointType.Foot;
feet.Add(nextJoint);
maxDistFromBodyJoint = 0f;
nextJoint = null;
}
else
{
nextJoint = nextJoint.nextJoints[0];// No need to worry about other next joints as only a body joint can have multiple next joints
}
}
}
else
{
bodyJointsToVisit.Add(nextJoint);
}
}
bodyJointsToVisit.RemoveAt(0);
}
if (baseOfNeck == null)
{
baseOfNeck = head;
}
for (int i = 0; i < feet.Count; i++)
{
BoneJoint currentBonejoint = feet[i];
float dist = 0;
while (currentBonejoint.previousJoint != null && currentBonejoint.isPartOfLeg())
{
currentBonejoint.attachedFoot = feet[i];
currentBonejoint.distanceFromFoot = dist;
currentBonejoint = currentBonejoint.previousJoint;
dist += currentBonejoint.distanceFromLastBone;
}
if (currentBonejoint != null && currentBonejoint.isPartOfBody())
{
currentBonejoint.type = BoneJoint.BoneJointType.Hip;
}
}
}
void FeedForward(Joint previous, Joint currentJoint)
{
if (previous.m_boneJoint == null || currentJoint.m_boneJoint == null)
{
return;
}
Vector2 previousJointPos = previous.transform.position;
Vector2 currentJointPos = currentJoint.transform.position;
Vector2 targetPos = GetPosLocked(previousJointPos, currentJointPos, currentJoint.m_boneJoint.distanceFromLastBone, false);
if (currentJoint.m_boneJoint.isPartOfLeg()) //is either a legjoint
{
if (currentJoint.m_boneJoint.nextJoints != null)
{
Vector3 footPos = currentJoint.m_boneJoint.attachedFoot.transform.position;
float maxDistFromFoot = currentJoint.m_boneJoint.distanceFromFoot;
float maxDist = currentJoint.m_boneJoint.maxDistanceFromBodyJoint + maxDistFromFoot;
float rotationToAddCurve = m_legCurve.Evaluate(maxDistFromFoot / maxDist);
float footToBodyAngle = Vector2.Angle(Vector2.right, (Vector2)(currentJoint.m_boneJoint.closestBodyJoint.transform.position - footPos));
Vector2 toAdd = new Vector2(0, maxDistFromFoot / maxDist);
Vector2 curved = (Quaternion.Euler(0, 0, footToBodyAngle * rotationToAddCurve) * toAdd) * m_legCurveStrength;
curved += currentJointPos;
Vector3 lockedPos = GetPosLocked(previousJointPos, curved, currentJoint.m_boneJoint.distanceFromLastBone, false);
targetPos = GetPosLocked(footPos, lockedPos, currentJoint.m_boneJoint.distanceFromFoot, true);
}
else //if is a foot
{
targetPos = currentJointPos;
}
}
else
{
if (currentJoint.m_boneJoint.type == BoneJoint.BoneJointType.Hip)
{
for (int i = 0; i < currentJoint.m_boneJoint.nextJoints.Count; i++)
{
BoneJoint nextJoint = currentJoint.m_boneJoint.nextJoints[i];
if (nextJoint.isPartOfLeg())
{
Vector2 lockedTo = nextJoint.attachedFoot.transform.position;
Vector2 jointMaxPos = GetPosLocked(lockedTo, nextJoint.transform.position, nextJoint.distanceFromFoot, true);
jointMaxPos = GetPosLocked(jointMaxPos, currentJointPos, nextJoint.distanceFromLastBone, false); //where the joint should be
targetPos = GetPosLocked(previousJointPos, jointMaxPos, currentJoint.m_boneJoint.distanceFromLastBone, false);
targetPos = GetPosLocked(lockedTo, targetPos, nextJoint.distanceFromFoot + nextJoint.distanceFromLastBone, true);
break;
}
}
}
else
{
BoneJoint nextHip = currentJoint.m_boneJoint;
float distFromHip = 0;
int i = 0;
while (nextHip.type != BoneJoint.BoneJointType.Hip && nextHip.nextJoints.Count > 0)
{
nextHip = nextHip.nextJoints[0];
distFromHip += nextHip.distanceFromLastBone;
}
Vector2 jointMaxPos = GetPosLocked(nextHip.transform.position, currentJointPos, distFromHip, true);
targetPos = GetPosLocked(previousJointPos, jointMaxPos, currentJoint.m_boneJoint.distanceFromLastBone, false);
}
}
currentJoint.transform.position = targetPos;//Vector2.MoveTowards(currentJoint.transform.position, targetPos, m_boneJointsLerpSpeed * Time.deltaTime);
UpdatePosToNeighbours(currentJoint, previous);
}