public override Vector3 ClosestPointOnNodeXZ(Vector3 _p) { INavmeshHolder navmeshHolder = TriangleMeshNode.GetNavmeshHolder(base.GraphIndex); Int3 vertex = navmeshHolder.GetVertex(this.v0); Int3 vertex2 = navmeshHolder.GetVertex(this.v1); Int3 vertex3 = navmeshHolder.GetVertex(this.v2); Int3 point = (Int3)_p; int y = point.y; vertex.y = 0; vertex2.y = 0; vertex3.y = 0; point.y = 0; if ((long)(vertex2.x - vertex.x) * (long)(point.z - vertex.z) - (long)(point.x - vertex.x) * (long)(vertex2.z - vertex.z) > 0L) { float num = Mathf.Clamp01(VectorMath.ClosestPointOnLineFactor(vertex, vertex2, point)); return(new Vector3((float)vertex.x + (float)(vertex2.x - vertex.x) * num, (float)y, (float)vertex.z + (float)(vertex2.z - vertex.z) * num) * 0.001f); } if ((long)(vertex3.x - vertex2.x) * (long)(point.z - vertex2.z) - (long)(point.x - vertex2.x) * (long)(vertex3.z - vertex2.z) > 0L) { float num2 = Mathf.Clamp01(VectorMath.ClosestPointOnLineFactor(vertex2, vertex3, point)); return(new Vector3((float)vertex2.x + (float)(vertex3.x - vertex2.x) * num2, (float)y, (float)vertex2.z + (float)(vertex3.z - vertex2.z) * num2) * 0.001f); } if ((long)(vertex.x - vertex3.x) * (long)(point.z - vertex3.z) - (long)(point.x - vertex3.x) * (long)(vertex.z - vertex3.z) > 0L) { float num3 = Mathf.Clamp01(VectorMath.ClosestPointOnLineFactor(vertex3, vertex, point)); return(new Vector3((float)vertex3.x + (float)(vertex.x - vertex3.x) * num3, (float)y, (float)vertex3.z + (float)(vertex.z - vertex3.z) * num3) * 0.001f); } return(_p); }
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; } } } } }
public static float NearestPointFactor(Vector3 lineStart, Vector3 lineEnd, Vector3 point) { return(VectorMath.ClosestPointOnLineFactor(lineStart, lineEnd, point)); }
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" })); } } } } } }
public override Vector3 ClosestPointOnNodeXZ(Vector3 _p) { // Get the object holding the vertex data for this node // This is usually a graph or a recast graph tile INavmeshHolder g = GetNavmeshHolder(GraphIndex); // Get all 3 vertices for this node Int3 tp1 = g.GetVertex(v0); Int3 tp2 = g.GetVertex(v1); Int3 tp3 = g.GetVertex(v2); // We need the point as an Int3 var p = (Int3)_p; // Save the original y coordinate, we will return a point with the same y coordinate int oy = p.y; // Assumes the triangle vertices are laid out in (counter?)clockwise order tp1.y = 0; tp2.y = 0; tp3.y = 0; p.y = 0; if ((long)(tp2.x - tp1.x) * (long)(p.z - tp1.z) - (long)(p.x - tp1.x) * (long)(tp2.z - tp1.z) > 0) { float f = Mathf.Clamp01(VectorMath.ClosestPointOnLineFactor(tp1, tp2, p)); return(new Vector3(tp1.x + (tp2.x - tp1.x) * f, oy, tp1.z + (tp2.z - tp1.z) * f) * Int3.PrecisionFactor); } else if ((long)(tp3.x - tp2.x) * (long)(p.z - tp2.z) - (long)(p.x - tp2.x) * (long)(tp3.z - tp2.z) > 0) { float f = Mathf.Clamp01(VectorMath.ClosestPointOnLineFactor(tp2, tp3, p)); return(new Vector3(tp2.x + (tp3.x - tp2.x) * f, oy, tp2.z + (tp3.z - tp2.z) * f) * Int3.PrecisionFactor); } else if ((long)(tp1.x - tp3.x) * (long)(p.z - tp3.z) - (long)(p.x - tp3.x) * (long)(tp1.z - tp3.z) > 0) { float f = Mathf.Clamp01(VectorMath.ClosestPointOnLineFactor(tp3, tp1, p)); return(new Vector3(tp3.x + (tp1.x - tp3.x) * f, oy, tp3.z + (tp1.z - tp3.z) * f) * Int3.PrecisionFactor); } else { return(_p); } /* * Equivalent to the above, but the above uses manual inlining * if (!VectorMath.RightOrColinearXZ (tp1, tp2, p)) { * float f = Mathf.Clamp01 (VectorMath.ClosestPointOnLineFactor (tp1, tp2, p)); * return new Vector3(tp1.x + (tp2.x-tp1.x)*f, oy, tp1.z + (tp2.z-tp1.z)*f)*Int3.PrecisionFactor; * } else if (!VectorMath.RightOrColinearXZ (tp2, tp3, p)) { * float f = Mathf.Clamp01 (VectorMath.ClosestPointOnLineFactor (tp2, tp3, p)); * return new Vector3(tp2.x + (tp3.x-tp2.x)*f, oy, tp2.z + (tp3.z-tp2.z)*f)*Int3.PrecisionFactor; * } else if (!VectorMath.RightOrColinearXZ (tp3, tp1, p)) { * float f = Mathf.Clamp01 (VectorMath.ClosestPointOnLineFactor (tp3, tp1, p)); * return new Vector3(tp3.x + (tp1.x-tp3.x)*f, oy, tp3.z + (tp1.z-tp3.z)*f)*Int3.PrecisionFactor; * } else { * return _p; * }*/ /* Almost equivalent to the above, but this is slower * Vector3 tp1 = (Vector3)g.GetVertex(v0); * Vector3 tp2 = (Vector3)g.GetVertex(v1); * Vector3 tp3 = (Vector3)g.GetVertex(v2); * tp1.y = 0; * tp2.y = 0; * tp3.y = 0; * _p.y = 0; * return Pathfinding.Polygon.ClosestPointOnTriangle (tp1,tp2,tp3,_p);*/ }
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" })); } } } } } } }