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; int graph = (int)startNode.GraphIndex; //Search all graphs but the one which start and end nodes are on nn.graphMask = ~(1 << graph); startNode.SetPosition((Int3)StartTransform.position); endNode.SetPosition((Int3)EndTransform.position); RemoveConnections(startNode); RemoveConnections(endNode); uint cost = (uint)Mathf.RoundToInt(((Int3)(StartTransform.position - EndTransform.position)).costMagnitude * costFactor); startNode.AddConnection(endNode, cost); endNode.AddConnection(startNode, cost); if (connectedNode1 == null || forceNewCheck) { NNInfo n1 = AstarPath.active.GetNearest(StartTransform.position, nn); connectedNode1 = n1.node as MeshNode; clamped1 = n1.clampedPosition; } if (connectedNode2 == null || forceNewCheck) { NNInfo n2 = AstarPath.active.GetNearest(EndTransform.position, nn); connectedNode2 = n2.node as MeshNode; clamped2 = n2.clampedPosition; } if (connectedNode2 == null || connectedNode1 == null) { return; } //Add connections between nodes, or replace old connections if existing connectedNode1.AddConnection(startNode, (uint)Mathf.RoundToInt(((Int3)(clamped1 - StartTransform.position)).costMagnitude * costFactor)); if (!oneWay) { connectedNode2.AddConnection(endNode, (uint)Mathf.RoundToInt(((Int3)(clamped2 - EndTransform.position)).costMagnitude * costFactor)); } if (!oneWay) { startNode.AddConnection(connectedNode1, (uint)Mathf.RoundToInt(((Int3)(clamped1 - StartTransform.position)).costMagnitude * costFactor)); } endNode.AddConnection(connectedNode2, (uint)Mathf.RoundToInt(((Int3)(clamped2 - EndTransform.position)).costMagnitude * costFactor)); }
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; } } } } }