Exemple #1
0
    public static VInt3 NearestPointStrict(ref VInt3 lineStart, ref VInt3 lineEnd, ref VInt3 point)
    {
        VInt3 rhs = lineEnd - lineStart;
        long  sqrMagnitudeLong = rhs.sqrMagnitudeLong;

        if (sqrMagnitudeLong == 0)
        {
            return(lineStart);
        }
        long m = IntMath.Clamp(VInt3.DotLong(point - lineStart, rhs), 0L, sqrMagnitudeLong);

        return(IntMath.Divide(rhs, m, sqrMagnitudeLong) + lineStart);
    }
Exemple #2
0
    public static VFactor NearestPointFactor(ref VInt3 lineStart, ref VInt3 lineEnd, ref VInt3 point)
    {
        VInt3   rhs = lineEnd - lineStart;
        long    sqrMagnitudeLong = rhs.sqrMagnitudeLong;
        VFactor zero             = VFactor.zero;

        zero.nom = VInt3.DotLong(point - lineStart, rhs);
        if (sqrMagnitudeLong != 0)
        {
            zero.den = sqrMagnitudeLong;
        }
        return(zero);
    }
Exemple #3
0
    /** Returns the closest point on the segment.
     * The segment is NOT treated as infinite.
     * \see ClosestPointOnLine
     * \see ClosestPointOnSegmentXZ
     */
    public static VInt3 ClosestPointOnSegment(VInt3 lineStart, VInt3 lineEnd, VInt3 point)
    {
        var  dir     = lineEnd - lineStart;
        long sqrMagn = dir.sqrMagnitudeLong;

        if (sqrMagn <= 1)
        {
            return(lineStart);
        }

        long factor = VInt3.DotLong(point - lineStart, dir) / sqrMagn;

        return(lineStart + dir * IntMath.Clamp(0, 1000, factor));
    }
        public static VInt3 NearestPointStrict(ref VInt3 lineStart, ref VInt3 lineEnd, ref VInt3 point)
        {
            VInt3 vInt             = lineEnd - lineStart;
            long  sqrMagnitudeLong = vInt.sqrMagnitudeLong;

            if (sqrMagnitudeLong == 0L)
            {
                return(lineStart);
            }
            long num = VInt3.DotLong(point - lineStart, vInt);

            num = IntMath.Clamp(num, 0L, sqrMagnitudeLong);
            return(IntMath.Divide(vInt, num, sqrMagnitudeLong) + lineStart);
        }
Exemple #5
0
        public void Apply(bool forceNewCheck)
        {
            //TODO
            //This function assumes that connections from the n1,n2 nodes never need to be removed in the future (e.g because the nodes move or something)
            NNConstraint nn = NNConstraint.None;

            nn.distanceXZ = true;
            int graph = (int)startNode.GraphIndex;

            //Search all graphs but the one which start and end nodes are on
            nn.graphMask = ~(1 << graph);

            bool same = true;

            {
                //Good Game
                //var info = AstarPath.active.GetNearest(StartTransform.position, nn);
                var info = AstarPath.active.GetNearest((VInt3)StartTransform.position, nn);
                same          &= info.node == connectedNode1 && info.node != null;
                connectedNode1 = info.node as MeshNode;
                clamped1       = info.position;
                if (connectedNode1 != null)
                {
                    Debug.DrawRay((Vector3)connectedNode1.position, Vector3.up * 5, Color.red);
                }
            }

            {
                //Good Game
                //var info = AstarPath.active.GetNearest(EndTransform.position, nn);
                var info = AstarPath.active.GetNearest((VInt3)EndTransform.position, nn);
                same          &= info.node == connectedNode2 && info.node != null;
                connectedNode2 = info.node as MeshNode;
                clamped2       = info.position;
                if (connectedNode2 != null)
                {
                    Debug.DrawRay((Vector3)connectedNode2.position, Vector3.up * 5, Color.cyan);
                }
            }

            if (connectedNode2 == null || connectedNode1 == null)
            {
                return;
            }

            startNode.SetPosition((VInt3)StartTransform.position);
            endNode.SetPosition((VInt3)EndTransform.position);

            if (same && !forceNewCheck)
            {
                return;
            }

            RemoveConnections(startNode);
            RemoveConnections(endNode);

            uint cost = (uint)Mathf.RoundToInt(((VInt3)(StartTransform.position - EndTransform.position)).costMagnitude * costFactor);

            startNode.AddConnection(endNode, cost);
            endNode.AddConnection(startNode, cost);

            VInt3 dir = connectedNode2.position - connectedNode1.position;

            for (int a = 0; a < connectedNode1.GetVertexCount(); a++)
            {
                VInt3 va1 = connectedNode1.GetVertex(a);
                VInt3 va2 = connectedNode1.GetVertex((a + 1) % connectedNode1.GetVertexCount());

                if (VInt3.DotLong((va2 - va1).Normal2D(), dir) > 0)
                {
                    continue;
                }

                for (int b = 0; b < connectedNode2.GetVertexCount(); b++)
                {
                    VInt3 vb1 = connectedNode2.GetVertex(b);
                    VInt3 vb2 = connectedNode2.GetVertex((b + 1) % connectedNode2.GetVertexCount());

                    if (VInt3.DotLong((vb2 - vb1).Normal2D(), dir) < 0)
                    {
                        continue;
                    }

                    if (VInt3.Angle((vb2 - vb1), (va2 - va1)) > (170.0 / 360.0f) * Mathf.PI * 2)
                    {
                        float t1 = 0;
                        float t2 = 1;

                        t2 = System.Math.Min(t2, VectorMath.ClosestPointOnLineFactor(va1, va2, vb1));
                        t1 = System.Math.Max(t1, VectorMath.ClosestPointOnLineFactor(va1, va2, vb2));

                        if (t2 < t1)
                        {
                            Debug.LogError("Something went wrong! " + t1 + " " + t2 + " " + va1 + " " + va2 + " " + vb1 + " " + vb2 + "\nTODO, how can this happen?");
                        }
                        else
                        {
                            //Good Game

                            /*Vector3 pa = (Vector3)(va2-va1)*t1 + (Vector3)va1;
                            *                           Vector3 pb = (Vector3)(va2-va1)*t2 + (Vector3)va1;*/
                            VInt3 pa = (va2 - va1) * t1 + va1;
                            VInt3 pb = (va2 - va1) * t2 + va1;

                            startNode.portalA = pa;
                            startNode.portalB = pb;

                            endNode.portalA = pb;
                            endNode.portalB = pa;

                            //Add connections between nodes, or replace old connections if existing
                            //Good Game

                            /*connectedNode1.AddConnection(startNode, (uint)Mathf.RoundToInt(((VInt3)(clamped1 - StartTransform.position)).costMagnitude*costFactor));
                             *                          connectedNode2.AddConnection(endNode, (uint)Mathf.RoundToInt(((VInt3)(clamped2 - EndTransform.position)).costMagnitude*costFactor));
                             *
                             *                          startNode.AddConnection(connectedNode1, (uint)Mathf.RoundToInt(((VInt3)(clamped1 - StartTransform.position)).costMagnitude*costFactor));
                             *                          endNode.AddConnection(connectedNode2, (uint)Mathf.RoundToInt(((VInt3)(clamped2 - EndTransform.position)).costMagnitude*costFactor));*/
                            connectedNode1.AddConnection(startNode, (uint)Mathf.RoundToInt(((clamped1 - (VInt3)StartTransform.position)).costMagnitude * costFactor));
                            connectedNode2.AddConnection(endNode, (uint)Mathf.RoundToInt(((clamped2 - (VInt3)EndTransform.position)).costMagnitude * costFactor));

                            startNode.AddConnection(connectedNode1, (uint)Mathf.RoundToInt(((clamped1 - (VInt3)StartTransform.position)).costMagnitude * costFactor));
                            endNode.AddConnection(connectedNode2, (uint)Mathf.RoundToInt(((clamped2 - (VInt3)EndTransform.position)).costMagnitude * costFactor));

                            return;
                        }
                    }
                }
            }
        }