Angle() public static method

public static Angle ( Int3 lhs, Int3 rhs ) : float
lhs Int3
rhs Int3
return float
コード例 #1
0
ファイル: NodeLink3.cs プロジェクト: moto2002/jiandangjianghu
        public void Apply(bool forceNewCheck)
        {
            NNConstraint none = NNConstraint.None;

            none.distanceXZ = true;
            int graphIndex = (int)this.startNode.GraphIndex;

            none.graphMask = ~(1 << graphIndex);
            bool   flag    = true;
            NNInfo nearest = AstarPath.active.GetNearest(this.StartTransform.position, none);

            flag &= (nearest.node == this.connectedNode1 && nearest.node != null);
            this.connectedNode1 = (nearest.node as MeshNode);
            this.clamped1       = nearest.clampedPosition;
            if (this.connectedNode1 != null)
            {
                Debug.DrawRay((Vector3)this.connectedNode1.position, Vector3.up * 5f, Color.red);
            }
            NNInfo nearest2 = AstarPath.active.GetNearest(this.EndTransform.position, none);

            flag &= (nearest2.node == this.connectedNode2 && nearest2.node != null);
            this.connectedNode2 = (nearest2.node as MeshNode);
            this.clamped2       = nearest2.clampedPosition;
            if (this.connectedNode2 != null)
            {
                Debug.DrawRay((Vector3)this.connectedNode2.position, Vector3.up * 5f, Color.cyan);
            }
            if (this.connectedNode2 == null || this.connectedNode1 == null)
            {
                return;
            }
            this.startNode.SetPosition((Int3)this.StartTransform.position);
            this.endNode.SetPosition((Int3)this.EndTransform.position);
            if (flag && !forceNewCheck)
            {
                return;
            }
            this.RemoveConnections(this.startNode);
            this.RemoveConnections(this.endNode);
            uint cost = (uint)Mathf.RoundToInt((float)((Int3)(this.StartTransform.position - this.EndTransform.position)).costMagnitude * this.costFactor);

            this.startNode.AddConnection(this.endNode, cost);
            this.endNode.AddConnection(this.startNode, cost);
            Int3 rhs = this.connectedNode2.position - this.connectedNode1.position;

            for (int i = 0; i < this.connectedNode1.GetVertexCount(); i++)
            {
                Int3 vertex  = this.connectedNode1.GetVertex(i);
                Int3 vertex2 = this.connectedNode1.GetVertex((i + 1) % this.connectedNode1.GetVertexCount());
                if (Int3.DotLong((vertex2 - vertex).Normal2D(), rhs) <= 0L)
                {
                    for (int j = 0; j < this.connectedNode2.GetVertexCount(); j++)
                    {
                        Int3 vertex3 = this.connectedNode2.GetVertex(j);
                        Int3 vertex4 = this.connectedNode2.GetVertex((j + 1) % this.connectedNode2.GetVertexCount());
                        if (Int3.DotLong((vertex4 - vertex3).Normal2D(), rhs) >= 0L)
                        {
                            if ((double)Int3.Angle(vertex4 - vertex3, vertex2 - vertex) > 2.9670598109563189)
                            {
                                float num  = 0f;
                                float num2 = 1f;
                                num2 = Math.Min(num2, VectorMath.ClosestPointOnLineFactor(vertex, vertex2, vertex3));
                                num  = Math.Max(num, VectorMath.ClosestPointOnLineFactor(vertex, vertex2, vertex4));
                                if (num2 >= num)
                                {
                                    Vector3 vector  = (Vector3)(vertex2 - vertex) * num + (Vector3)vertex;
                                    Vector3 vector2 = (Vector3)(vertex2 - vertex) * num2 + (Vector3)vertex;
                                    this.startNode.portalA = vector;
                                    this.startNode.portalB = vector2;
                                    this.endNode.portalA   = vector2;
                                    this.endNode.portalB   = vector;
                                    this.connectedNode1.AddConnection(this.startNode, (uint)Mathf.RoundToInt((float)((Int3)(this.clamped1 - this.StartTransform.position)).costMagnitude * this.costFactor));
                                    this.connectedNode2.AddConnection(this.endNode, (uint)Mathf.RoundToInt((float)((Int3)(this.clamped2 - this.EndTransform.position)).costMagnitude * this.costFactor));
                                    this.startNode.AddConnection(this.connectedNode1, (uint)Mathf.RoundToInt((float)((Int3)(this.clamped1 - this.StartTransform.position)).costMagnitude * this.costFactor));
                                    this.endNode.AddConnection(this.connectedNode2, (uint)Mathf.RoundToInt((float)((Int3)(this.clamped2 - this.EndTransform.position)).costMagnitude * this.costFactor));
                                    return;
                                }
                                Debug.LogError(string.Concat(new object[]
                                {
                                    "Wait wut!? ",
                                    num,
                                    " ",
                                    num2,
                                    " ",
                                    vertex,
                                    " ",
                                    vertex2,
                                    " ",
                                    vertex3,
                                    " ",
                                    vertex4,
                                    "\nTODO, fix this error"
                                }));
                            }
                        }
                    }
                }
            }
        }
コード例 #2
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;

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

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

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

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

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

            RemoveConnections(startNode);
            RemoveConnections(endNode);

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

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

            Int3 dir = connectedNode2.position - connectedNode1.position;

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

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

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

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


                    //Debug.DrawLine ((Vector3)va1, (Vector3)va2, Color.magenta);
                    //Debug.DrawLine ((Vector3)vb1, (Vector3)vb2, Color.cyan);
                    //Debug.Break ();

                    if (Int3.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("Wait wut!? " + t1 + " " + t2 + " " + va1 + " " + va2 + " " + vb1 + " " + vb2 + "\nTODO, fix this error");
                        }
                        else
                        {
                            Vector3 pa = (Vector3)(va2 - va1) * t1 + (Vector3)va1;
                            Vector3 pb = (Vector3)(va2 - va1) * t2 + (Vector3)va1;

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

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

                            //Add connections between nodes, or replace old connections if existing
                            connectedNode1.AddConnection(startNode, (uint)Mathf.RoundToInt(((Int3)(clamped1 - StartTransform.position)).costMagnitude * costFactor));
                            connectedNode2.AddConnection(endNode, (uint)Mathf.RoundToInt(((Int3)(clamped2 - EndTransform.position)).costMagnitude * costFactor));

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

                            return;
                        }
                    }
                }
            }
        }
コード例 #3
0
        public void Apply(bool forceNewCheck)
        {
            NNConstraint none = NNConstraint.None;

            none.distanceXZ = true;
            int graphIndex = (int)this.startNode.GraphIndex;

            none.graphMask = ~(((int)1) << graphIndex);
            bool   flag    = true;
            NNInfo nearest = AstarPath.active.GetNearest(this.StartTransform.position, none);

            flag &= (nearest.node == this.connectedNode1) && (nearest.node != null);
            this.connectedNode1 = nearest.node as MeshNode;
            this.clamped1       = nearest.position;
            if (this.connectedNode1 != null)
            {
                Debug.DrawRay((Vector3)this.connectedNode1.position, (Vector3)(Vector3.up * 5f), Color.red);
            }
            NNInfo info2 = AstarPath.active.GetNearest(this.EndTransform.position, none);

            flag &= (info2.node == this.connectedNode2) && (info2.node != null);
            this.connectedNode2 = info2.node as MeshNode;
            this.clamped2       = info2.position;
            if (this.connectedNode2 != null)
            {
                Debug.DrawRay((Vector3)this.connectedNode2.position, (Vector3)(Vector3.up * 5f), Color.cyan);
            }
            if ((this.connectedNode2 != null) && (this.connectedNode1 != null))
            {
                this.startNode.SetPosition((Int3)this.StartTransform.position);
                this.endNode.SetPosition((Int3)this.EndTransform.position);
                if (!flag || forceNewCheck)
                {
                    this.RemoveConnections(this.startNode);
                    this.RemoveConnections(this.endNode);
                    Int3 num12 = (Int3)(this.StartTransform.position - this.EndTransform.position);
                    uint cost  = (uint)Mathf.RoundToInt(num12.costMagnitude * this.costFactor);
                    this.startNode.AddConnection(this.endNode, cost);
                    this.endNode.AddConnection(this.startNode, cost);
                    Int3 rhs = this.connectedNode2.position - this.connectedNode1.position;
                    for (int i = 0; i < this.connectedNode1.GetVertexCount(); i++)
                    {
                        Int3 vertex  = this.connectedNode1.GetVertex(i);
                        Int3 lineEnd = this.connectedNode1.GetVertex((i + 1) % this.connectedNode1.GetVertexCount());
                        Int3 num13   = lineEnd - vertex;
                        if (Int3.DotLong(num13.Normal2D(), rhs) <= 0L)
                        {
                            for (int j = 0; j < this.connectedNode2.GetVertexCount(); j++)
                            {
                                Int3 point = this.connectedNode2.GetVertex(j);
                                Int3 num9  = this.connectedNode2.GetVertex((j + 1) % this.connectedNode2.GetVertexCount());
                                Int3 num14 = num9 - point;
                                if ((Int3.DotLong(num14.Normal2D(), rhs) >= 0L) && (Int3.Angle(num9 - point, lineEnd - vertex) > 2.9670598109563189))
                                {
                                    float num10 = 0f;
                                    float num11 = 1f;
                                    num11 = Math.Min(num11, VectorMath.ClosestPointOnLineFactor(vertex, lineEnd, point));
                                    num10 = Math.Max(num10, VectorMath.ClosestPointOnLineFactor(vertex, lineEnd, num9));
                                    if (num11 >= num10)
                                    {
                                        Vector3 vector  = ((Vector3)(((Vector3)(lineEnd - vertex)) * num10)) + ((Vector3)vertex);
                                        Vector3 vector2 = ((Vector3)(((Vector3)(lineEnd - vertex)) * num11)) + ((Vector3)vertex);
                                        this.startNode.portalA = vector;
                                        this.startNode.portalB = vector2;
                                        this.endNode.portalA   = vector2;
                                        this.endNode.portalB   = vector;
                                        Int3 num15 = (Int3)(this.clamped1 - this.StartTransform.position);
                                        this.connectedNode1.AddConnection(this.startNode, (uint)Mathf.RoundToInt(num15.costMagnitude * this.costFactor));
                                        Int3 num16 = (Int3)(this.clamped2 - this.EndTransform.position);
                                        this.connectedNode2.AddConnection(this.endNode, (uint)Mathf.RoundToInt(num16.costMagnitude * this.costFactor));
                                        Int3 num17 = (Int3)(this.clamped1 - this.StartTransform.position);
                                        this.startNode.AddConnection(this.connectedNode1, (uint)Mathf.RoundToInt(num17.costMagnitude * this.costFactor));
                                        Int3 num18 = (Int3)(this.clamped2 - this.EndTransform.position);
                                        this.endNode.AddConnection(this.connectedNode2, (uint)Mathf.RoundToInt(num18.costMagnitude * this.costFactor));
                                        return;
                                    }
                                    Debug.LogError(string.Concat(new object[] { "Wait wut!? ", num10, " ", num11, " ", vertex, " ", lineEnd, " ", point, " ", num9, "\nTODO, fix this error" }));
                                }
                            }
                        }
                    }
                }
            }
        }