//--------------------------------------------------
        // 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);
        }
Exemple #3
0
        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();
            }
        }
Exemple #5
0
        //--------------------------------------------------
        //  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;
                }
            }
        }
Exemple #6
0
        //==================================================
        //  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);
            }
        }