void JointPathfinding()
{
int turnsFound = 0;
IK_Joint previousTargetedJoint = null;
Vector3 previousTurnPos = transform.position;
Pathfinding.ForEachTurn(grid.path, (Vector2 turnPos, Vector2 previousDir, Vector2 currentDir) => {
float distanceToTurn = 0;
float distanceToJoint = 0;
int targetJointIndex = 0;
//if (turnsFound == 0) {
distanceToTurn = ((Vector3)turnPos - previousTurnPos).magnitude;
// Find joint that can reach turn
for (int i = 0; i < joints.Count; i++)
{
distanceToJoint = i * segmentLength;
if (distanceToJoint >= distanceToTurn)
{
targetJointIndex = i;
break;
}
}
print("Turn_" + turnsFound + " -> Dst: " + distanceToTurn + ", DstJt: " + distanceToJoint + ", Joint#: " + targetJointIndex);
joints[targetJointIndex].SetTarget(turnPos);
//}
Solve(joints[targetJointIndex], false);
turnsFound++;
previousTurnPos = (Vector3)turnPos;
});
}
// Calls a function for all end joints from a starting joint
public static void ForAllEnds(IK_Joint startingJoint, JointCallback func)
{
if (startingJoint == null)
{
return;
}
if (startingJoint.jointType == JointTypeEnum.End)
{
func(startingJoint); // Call function for first joint
}
else
{
foreach (IK_Joint joint in startingJoint.childrenJoints)
{
if (joint.jointType == JointTypeEnum.End)
{
func(joint);
}
if (joint.childCount > 0) // If has children, call function on them too
{
ForAllEnds(joint, func);
}
}
}
}
// Use this for initialization
void Start()
{
//// Create Segments
//segments.Add(new IK_Segment(Vector3.zero, 2));
//segments.Add(new IK_Segment(Vector3.zero, 2));
//segments.Add(new IK_Segment(Vector3.zero, 2));
//// Connect
//segments[0].AddSegment(segments[1]);
//segments[0].AddSegment(segments[2]);
//root_1 = new IK_Joint(null, segmentLength);
//root_2 = new IK_Joint(root_1, segmentLength);
//fork_1 = new IK_Joint(root_2, segmentLength);
//left_1 = new IK_Joint(fork_1, segmentLength);
//left_2 = new IK_Joint(left_1, segmentLength);
//left_3 = new IK_Joint(left_2, segmentLength);
//left_end = new IK_Joint(left_3, segmentLength);
//right_1 = new IK_Joint(fork_1, segmentLength);
//right_end = new IK_Joint(right_1, segmentLength);
// Generated Joints
joints.Add(new IK_Joint(null, segmentLength));
for (int i = 0; i < extraJoints; i++)
{
joints.Add(new IK_Joint(joints[joints.Count - 1], segmentLength));
}
endJoint = joints[joints.Count - 1];
rootJoint = joints[0];
}
public void SolveJoint(IK_Joint joint, Vector3 target)
{
TargetAndSolve(joint, target);
List <IK_Joint> path_joint_to_end = GetJointPath(joint, endJoint); // Hard coded end joint
ForwardsAdjust(path_joint_to_end, joint.worldPosition);
}
public void AddJoint()
{
IK_Joint newJoint = new IK_Joint(joints[joints.Count - 1], jointSpacing);
//newJoint.worldPosition = root.worldPosition + (Vector3.right * jointCount * jointSpacing); // Initialize position
end = newJoint;
joints.Add(newJoint);
jointCount++;
}
public IK_Segment(Vector3 initialPos, int extraInitialJoints = 0)
{
root = new IK_Joint(null);
root.worldPosition = initialPos;
end = root;
joints.Add(root);
for (int i = 0; i < extraInitialJoints; i++)
{
AddJoint();
}
}
// Use this for initialization
void Start()
{
// Generated Joints
joints.Add(new IK_Joint(null, segmentLength));
for (int i = 0; i < extraJoints; i++)
{
joints.Add(new IK_Joint(joints[joints.Count - 1], segmentLength));
}
endJoint = joints[joints.Count - 1];
rootJoint = joints[0];
midJoint = joints[joints.Count / 2];
}
// --- NEW ----------------------------
public List <IK_Joint> GetJointPath(IK_Joint earlier, IK_Joint later)
{
List <IK_Joint> path = new List <IK_Joint>();
IK_Joint currentJoint = earlier;
while (currentJoint != later)
{
path.Add(currentJoint);
currentJoint = currentJoint.childrenJoints[0]; // This isn't going to work if there are forks!
}
path.Add(later);
return(path);
}
// -------------------------------------------------------------
// GET AN ARRAY OF JOINTS FROM THE ROOT TO THE TARGET JOINT \
// ---------------------------------------------------------------
public List <IK_Joint> GetFullPath()
{
List <IK_Joint> path = new List <IK_Joint>(new IK_Joint[limbIndex + 1]); // Size
path[limbIndex] = this;
IK_Joint currentJoint = parentJoint;
while (currentJoint.parentJoint != null)
{
path[currentJoint.limbIndex] = currentJoint;
currentJoint = currentJoint.parentJoint;
}
path[0] = currentJoint; // Add the root to the array
return(path);
}
// A way to call a function for all joints branching off of a starting joint (including the starting joint)
public void ForAllJoints(IK_Joint startingJoint, JointCallback func)
{
if (startingJoint == null)
{
return;
}
func(startingJoint); // Call function for first joint
foreach (IK_Joint joint in startingJoint.childrenJoints)
{
func(joint);
if (joint.childCount > 0) // If has children, call function on them too
{
ForAllJoints(joint, func);
}
}
}
// Automatically updates the details of joints according to the list of joints
public void UpdateJointDetailsInList()
{
IK_Joint parentSegmentJointConnector = joints[0].parentJoint;
IK_Joint[] childrenSegmentJointConnectors = new IK_Joint[childrenSegments.Count];
for (int i = 0; i < childrenSegments.Count; i++)
{
childrenSegmentJointConnectors[i] = childrenSegments[i].joints[0];
}
// Set up connections with root joint
for (int i = 0; i < joints.Count; i++)
{
}
}
// -------------------------------------------------------------
// GET AN ARRAY OF JOINTS FROM A FORK TO THE TARGET JOINT \
// ---------------------------------------------------------------
public List <IK_Joint> GetPathFromFork()
{
List <IK_Joint> path = new List <IK_Joint>(new IK_Joint[distanceFromLastFork + 1]);
path[distanceFromLastFork] = this;
if (this.jointType == JointTypeEnum.Root)
{
return(path);
}
IK_Joint currentJoint = parentJoint;
while (currentJoint.jointType != JointTypeEnum.Fork && currentJoint.jointType != JointTypeEnum.Root)
{
path[currentJoint.distanceFromLastFork] = currentJoint;
currentJoint = currentJoint.parentJoint;
}
path[0] = currentJoint; // Add the fork/root to the array
return(path);
}
public void Solve(IK_Joint endPoint, bool requiredInRange)
{
Vector3 target = endPoint.target;
int iterationNum = 0;
float error = 0;
List <IK_Joint> path = endPoint.GetPathFromFork();
// Check if can reach
if (requiredInRange)
{
float totalLimbDistance = 0;
float requiredDistance = 0;
foreach (IK_Joint joint in path)
{
totalLimbDistance += joint.distanceToParent;
if (joint.jointType == IK_Joint.JointTypeEnum.Root)
{
requiredDistance = (joint.worldPosition - target).magnitude;
}
}
if (totalLimbDistance < requiredDistance)
{
return;
}
}
// Start adjusting the positions of joints
do
{
Vector3 originPos = path[0].worldPosition; // Used later
BackwardsAdjust(path, target); // Backwards through the joints
ForwardsAdjust(path, originPos); // Forwards through the joints
error = (path[path.Count - 1].worldPosition - target).magnitude;
iterationNum++;
} while (iterationNum < ITERATION_COUNT && (error > MARGIN_OF_ERROR && iterationNum != 0));
// While iteration num is below ITERATION_COUNT and the MARGIN_OF_ERROR hasn't been reached yet...
}
public IK_Joint(IK_Joint _parentJoint, float _distanceToParent = 0)
{
id = joint_id;
joint_id++;
parentJoint = _parentJoint;
if (_parentJoint == null) // If there is no parent (null), return early
{
return;
}
distanceToParent = _distanceToParent;
distanceToRoot = _parentJoint.distanceToRoot + 1;
_parentJoint.AddChild(this);
childIndex = _parentJoint.childrenJoints.Count - 1;
limbIndex = _parentJoint.limbIndex + 1;
ForAllChildren(_parentJoint, (IK_Joint joint) => {
joint.UpdateJointDetails();
});
}
// -------------------------------------
// ADDS A CHILD TO THE JOINT \
// ---------------------------------------
public void AddChild(IK_Joint newJoint)
{
childrenJoints.Add(newJoint);
childCount++;
}
public void TargetAndSolve(IK_Joint joint, Vector3 target)
{
joint.SetTarget(target);
Solve(joint, false);
ForwardsAdjust(GetJointPath(joint, endJoint), joint.worldPosition); // Pull on parent joints
}
