//--------------------------------------------------
// Forward motion means moving from node A to node B,
// B to A for backwards motion. Next segment is found
// by matching the end node to the beginning node of
// another segment
//--------------------------------------------------
public WPSegment GetNextSeg(WPSegment curSeg, bool reverse)
{
WPNode curNode = (!reverse) ? curSeg.nodeB : curSeg.nodeA; // Cache the end node of the first segment
WPSegment nextSeg = null;
foreach (var s in segments)
{
if (!reverse)
{
if (s.nodeA == curNode)
{
nextSeg = s;
break;
}
}
else
{
if (s.nodeB == curNode)
{
nextSeg = s;
break;
}
}
}
if (nextSeg == null)
{
Debug.Log("Next segment not found. Path is complete or destroyed.");
}
return(nextSeg);
}
//--------------------------------------------------
// Get a new segment from the pool.
//--------------------------------------------------
public WPSegment GetSeg(WPNode nodeA, WPNode nodeB, Vector3[] subNodes)
{
WPSegment returnVal = null;
for (int i = 0; i < pool.Count; ++i)
{
// If a segment with the same start and end node is found in the pool,
// return that. No need to make a new object.
if (pool[i].nodeA == nodeA && pool[i].nodeB == nodeB)
{
returnVal = pool[i];
break;
}
// Otherwise, check to see if there is another pool object that's ready
// to be recycled (dead) in the pool. If so, use that.
if (pool[i].isDead)
{
returnVal = pool[i];
}
}
if (returnVal == null) // Only if there are no matching or dead segments, create a new object.
{
returnVal = new WPSegment(nodeA, nodeB, subNodes);
//Debug.Log("new segment created");
pool.Add(returnVal);
}
else // Otherwise populate the recycled object with the desired values.
{
returnVal.nodeA = nodeA;
returnVal.nodeB = nodeB;
returnVal.subNodes = subNodes;
}
return(returnVal);
}
void TraverseFromClosestPoint()
{
WPSegment seg;
int sub;
controller.GetNearestSegAndSub(transform.position, out seg, out sub);
curSeg = seg;
curSub = sub;
traversing = true;
}
public void GetNearestSegAndSub(Vector3 traverser, out WPSegment seg, out int sub)
{
float nearestSqDist = float.MaxValue;
seg = null;
sub = 0;
foreach (var s in segments)
{
for (int i = 0; i < s.subNodes.Length; ++i)
{
float sqDistFromSub = (s.subNodes[i] - traverser).sqrMagnitude;
if (sqDistFromSub < nearestSqDist)
{
seg = s;
sub = i;
nearestSqDist = sqDistFromSub;
}
}
}
if (seg == null)
{
seg = GetFirstSeg();
}
}
//--------------------------------------------------
// Main traversal loop
//--------------------------------------------------
void TraverseLoop()
{
if (curSeg == null)
{
return;
}
int lastSubNode = curSeg.subNodes.Length - 1;
Vector3 curPos = (useRigidbody) ? rb.position : transform.position;
bool willOvershoot = false;
// Prevent unforseen endless loop
float failsafe = 0.0f;
const float failsafeTime = 0.5f;
// if movement method returns that speed will overshoot,
// keep running through segments until it no longer will.
do
{
Vector3 dest = controller.transform.position + curSeg.subNodes[curSub];
willOvershoot = MoveBySpeed(dest, moveSpeed, ref curPos);
if (willOvershoot)
{
curSub += (reverse) ? -1 : 1;
}
if (curSub < 0 || curSub > lastSubNode)
{
WPNode curNode = (!reverse) ? curSeg.nodeB : curSeg.nodeA;
if (curNode.changeDirection)
{
reverse = !reverse; // if changing direction, no need to find new seg, just reset subnode
}
else
{
curSeg = controller.GetNextSeg(curSeg, reverse);
}
if (curSeg == null)
{
Debug.Log("Traversal complete: " + gameObject.name);
traversing = false;
return;
}
curSub = (reverse) ? lastSubNode - 1 : 1;
}
failsafe += Time.deltaTime;
} while (willOvershoot && failsafe < failsafeTime);
if (!willOvershoot)
{
if (useRigidbody)
{
rb.MovePosition(curPos);
}
else
{
transform.position = curPos;
}
}
}
//==================================================
// PRIVATE METHODS
//==================================================
void TraverseFromFirstNode()
{
curSeg = controller.GetFirstSeg();
curSub = 0;
traversing = true;
}
//==================================================
// PRIVATE METHODS
//==================================================
//--------------------------------------------------
// Populates the segment list with end node objects
// and the subnode list, according to the desired
// resolution. All positions in a segment are in
// local space relative to the controller object.
//--------------------------------------------------
void BuildSegmentList(List <WPNode> activeList)
{
WPNode[] nodes = activeList.ToArray();
segments.Clear();
segPool.Clean();
if (nodes.Length < 2)
{
Debug.LogError("Active Node List is of invalid size (less than 2 WPNodes)");
return;
}
int length = (isLooping) ? nodes.Length : nodes.Length - 1; // Do not build segment for the last node if not looping
Vector3 mp1 = Vector3.zero;
Vector3 mp2 = Vector3.zero;
Vector3 mp3 = Vector3.zero;
Vector3 e1 = Vector3.zero;
Vector3 e2 = Vector3.zero;
for (int index = 0; index < length; index++)
{
/*--------------------------------------------
* Find 4 points for cubic bezier describing 3
* segments, duplicating first and last if at
* ends.
* --------------------------------------------*/
int indexMinus1 = index - 1;
int indexPlus1 = index + 1;
int indexPlus2 = index + 2;
if (isLooping)
{
indexMinus1 = (indexMinus1 < 0) ? nodes.Length - 1 : 0;
indexPlus1 = indexPlus1 % nodes.Length;
indexPlus2 = indexPlus2 % nodes.Length;
}
indexMinus1 = Mathf.Clamp(indexMinus1, 0, nodes.Length - 1);
indexPlus1 = Mathf.Clamp(indexPlus1, 0, nodes.Length - 1);
indexPlus2 = Mathf.Clamp(indexPlus2, 0, nodes.Length - 1);
Vector3 p1 = nodes[indexMinus1].transform.localPosition;
Vector3 p2 = nodes[index].transform.localPosition;
Vector3 p3 = nodes[indexPlus1].transform.localPosition;
Vector3 p4 = nodes[indexPlus2].transform.localPosition;
/*--------------------------------------------
* Find midpoints of 3 segments. Wrapped in an
* if statement, since there is no reason to
* recalculate the values; I am reusing 2 out of
* 3 of the values the next iteration.
* --------------------------------------------*/
if (index == 0)
{
mp1 = Vector3.Lerp(p1, p2, 0.5f);
mp2 = Vector3.Lerp(p2, p3, 0.5f);
}
else
{
mp1 = mp2;
mp2 = mp3;
}
mp3 = Vector3.Lerp(p3, p4, 0.5f);
/*--------------------------------------------
* Find point along midpoint connecting vectors,
* using relative scale of sides. Again, if this
* is the first iteration, calculate values,
* otherwise just reuse.
* --------------------------------------------*/
if (index == 0)
{
e1 = Vector3.Lerp(mp1, mp2, Vector3.Distance(p1, p2) / (Vector3.Distance(p1, p2) + Vector3.Distance(p2, p3)));
}
else
{
e1 = e2;
}
float dist = Vector3.Distance(p2, p3); // To avoid doing distance operation twice next line
e2 = Vector3.Lerp(mp2, mp3, dist / (dist + Vector3.Distance(p3, p4)));
/*--------------------------------------------
* Calculate control points of bezier curve.
* s1 and s4 are commented out because, while I
* could store the values as above, it's really
* not worth if for a simple operation.
*
* //Vector3 s1 = p2 + (mp1 - e1);
* //Vector3 s4 = p3 + (mp3 - e2);
* --------------------------------------------*/
Vector3 s2 = p2 + ((mp2 - e1) * nodes[index].shape);
Vector3 s3 = p3 + ((mp2 - e2) * nodes[indexPlus1].shape);
/*--------------------------------------------
* Iterate through and calculate all the points!
* --------------------------------------------*/
float x, y, z;
Vector3[] subnodes = new Vector3[resolution + 2]; // +2 for the start and end nodes
subnodes[0] = nodes[index].transform.localPosition; // Start node
subnodes[resolution + 1] = nodes[indexPlus1].transform.localPosition; // End node
//--------------------------------------------------
// Find intermediate points
//--------------------------------------------------
for (int k = 0; k < resolution; k++)
{
// t cannot be 0 or 1, or else the subnode ends up on the real node.
// So, in order to make all subnodes between real nodes, we are faking a higher
// resolution.
float t = (float)(k + 1) / (resolution + 1);
// Alas, I don't understand this next formula. Courtesy of http://pomax.github.io/bezierinfo/
// These are the equations for actually calculating the Bezier points.
x = p2.x * (1 - t).Pow(3) + s2.x * 3 * (1 - t).Pow(2) * t + s3.x * 3 * (1 - t) * t.Pow(2) + p3.x * t.Pow(3);
y = p2.y * (1 - t).Pow(3) + s2.y * 3 * (1 - t).Pow(2) * t + s3.y * 3 * (1 - t) * t.Pow(2) + p3.y * t.Pow(3);
z = p2.z * (1 - t).Pow(3) + s2.z * 3 * (1 - t).Pow(2) * t + s3.z * 3 * (1 - t) * t.Pow(2) + p3.z * t.Pow(3);
// If we have reached the destination node, assign this node to our Subnode
// start at index k + 1 to leave index 0 and (length - 1) alone, which are our end node positions.
subnodes[k + 1] = new Vector3(x, y, z);
}
WPSegment newSeg = segPool.GetSeg(nodes[index], nodes[indexPlus1], subnodes);
segments.Add(newSeg);
}
}
