/** This performs a linear search through all polygons returning the closest one */ public static NNInfo GetNearestForce(Node[] nodes, Int3[] vertices, Vector3 position, NNConstraint constraint) { Int3 pos = (Int3)position; //Replacement for Infinity, the maximum value a int can hold int minDist = -1; Node minNode = null; float minDist2 = -1; Node minNode2 = null; int minConstDist = -1; Node minNodeConst = null; float minConstDist2 = -1; Node minNodeConst2 = null; //int rnd = (int)Random.Range (0,10000); //int skipped = 0; for (int i = 0; i < nodes.Length; i++) { MeshNode node = nodes[i] as MeshNode; if (!Polygon.IsClockwise(vertices[node.v1], vertices[node.v2], pos) || !Polygon.IsClockwise(vertices[node.v2], vertices[node.v3], pos) || !Polygon.IsClockwise(vertices[node.v3], vertices[node.v1], pos)) { //Polygon.TriangleArea2 (vertices[node.v1],vertices[node.v2],pos) >= 0 || Polygon.TriangleArea2 (vertices[node.v2],vertices[node.v3],pos) >= 0 || Polygon.TriangleArea2 (vertices[node.v3],vertices[node.v1],pos) >= 0) { /*if (minDist2 != -1) { * float d1 = (node.position-vertices[node.v1]).sqrMagnitude; * d1 = Mathf.Min (d1,(node.position-vertices[node.v1]).sqrMagnitude); * d1 = Mathf.Min (d1,(node.position-vertices[node.v1]).sqrMagnitude); * * //The closest distance possible from the current node to 'pos' * d1 = (node.position-pos).sqrMagnitude-d1; * * if (d1 > minDist2) { * skipped++; * continue; * } * }*/ /*float dist2 = Mathfx.DistancePointSegment2 (pos.x,pos.z,vertices[node.v1].x,vertices[node.v1].z,vertices[node.v2].x,vertices[node.v2].z); * dist2 = Mathfx.Min (dist2,Mathfx.DistancePointSegment2 (pos.x,pos.z,vertices[node.v1].x,vertices[node.v1].z,vertices[node.v3].x,vertices[node.v3].z)); * dist2 = Mathfx.Min (dist2,Mathfx.DistancePointSegment2 (pos.x,pos.z,vertices[node.v3].x,vertices[node.v3].z,vertices[node.v2].x,vertices[node.v2].z));*/ float dist2 = (node.position - pos).sqrMagnitude; if (minDist2 == -1 || dist2 < minDist2) { minDist2 = dist2; minNode2 = node; } if (constraint.Suitable(node)) { if (minConstDist2 == -1 || dist2 < minConstDist2) { minConstDist2 = dist2; minNodeConst2 = node; } } continue; } int dist = Mathfx.Abs(node.position.y - pos.y); if (minDist == -1 || dist < minDist) { minDist = dist; minNode = node; } if (constraint.Suitable(node)) { if (minConstDist == -1 || dist < minConstDist) { minConstDist = dist; minNodeConst = node; } } } NNInfo nninfo = new NNInfo(minNode == null ? minNode2 : minNode, minNode == null ? NearestNodePriority.Low : NearestNodePriority.High); //Find the point closest to the nearest triangle //if (minNode == null) { if (nninfo.node != null) { MeshNode node = nninfo.node as MeshNode; //minNode2 as MeshNode; Vector3[] triangle = new Vector3[3] { vertices[node.v1], vertices[node.v2], vertices[node.v3] }; Vector3 clP = Polygon.ClosesPointOnTriangle(triangle, position); nninfo.clampedPosition = clP; } nninfo.constrainedNode = minNodeConst == null ? minNodeConst2 : minNodeConst; if (nninfo.constrainedNode != null) { MeshNode node = nninfo.constrainedNode as MeshNode; //minNode2 as MeshNode; Vector3[] triangle = new Vector3[3] { vertices[node.v1], vertices[node.v2], vertices[node.v3] }; Vector3 clP = Polygon.ClosesPointOnTriangle(triangle, position); nninfo.constClampedPosition = clP; } return(nninfo); }
/** This performs a linear search through all polygons returning the closest one. * This will fill the NNInfo with .node for the closest node not necessarily complying with the NNConstraint, and .constrainedNode with the closest node * complying with the NNConstraint. * \see GetNearestForce(Node[],Int3[],Vector3,NNConstraint,bool) */ public static NNInfo GetNearestForceBoth(Node[] nodes, Int3[] vertices, Vector3 position, NNConstraint constraint, bool accurateNearestNode) { Int3 pos = (Int3)position; float minDist = -1; Node minNode = null; float minConstDist = -1; Node minConstNode = null; float maxDistSqr = constraint.constrainDistance ? AstarPath.active.maxNearestNodeDistanceSqr : float.PositiveInfinity; if (nodes == null || nodes.Length == 0) { return(new NNInfo()); } for (int i = 0; i < nodes.Length; i++) { MeshNode node = nodes[i] as MeshNode; if (accurateNearestNode) { Vector3 closest = Polygon.ClosestPointOnTriangle((Vector3)vertices[node.v1], (Vector3)vertices[node.v2], (Vector3)vertices[node.v3], position); float dist = ((Vector3)pos - closest).sqrMagnitude; if (minNode == null || dist < minDist) { minDist = dist; minNode = node; } if (dist < maxDistSqr && constraint.Suitable(node)) { if (minConstNode == null || dist < minConstDist) { minConstDist = dist; minConstNode = node; } } } else { if (!Polygon.IsClockwise(vertices[node.v1], vertices[node.v2], pos) || !Polygon.IsClockwise(vertices[node.v2], vertices[node.v3], pos) || !Polygon.IsClockwise(vertices[node.v3], vertices[node.v1], pos)) { float dist = (node.position - pos).sqrMagnitude; if (minNode == null || dist < minDist) { minDist = dist; minNode = node; } if (dist < maxDistSqr && constraint.Suitable(node)) { if (minConstNode == null || dist < minConstDist) { minConstDist = dist; minConstNode = node; } } } else { int dist = Mathfx.Abs(node.position.y - pos.y); if (minNode == null || dist < minDist) { minDist = dist; minNode = node; } if (dist < maxDistSqr && constraint.Suitable(node)) { if (minConstNode == null || dist < minConstDist) { minConstDist = dist; minConstNode = node; } } } } } NNInfo nninfo = new NNInfo(minNode); //Find the point closest to the nearest triangle if (nninfo.node != null) { MeshNode node = nninfo.node as MeshNode; //minNode2 as MeshNode; Vector3 clP = Polygon.ClosestPointOnTriangle((Vector3)vertices[node.v1], (Vector3)vertices[node.v2], (Vector3)vertices[node.v3], position); nninfo.clampedPosition = clP; } nninfo.constrainedNode = minConstNode; if (nninfo.constrainedNode != null) { MeshNode node = nninfo.constrainedNode as MeshNode; //minNode2 as MeshNode; Vector3 clP = Polygon.ClosestPointOnTriangle((Vector3)vertices[node.v1], (Vector3)vertices[node.v2], (Vector3)vertices[node.v3], position); nninfo.constClampedPosition = clP; } return(nninfo); }