/** 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); }
/** Returns the closest point on the triangle. \note Got code from the internet, changed a bit to work with the Unity API * \todo Uses Dot product to get the sqrMagnitude of a vector, should change to sqrMagnitude for readability and possibly for speed (unlikely though) */ public static Vector3 ClosesPointOnTriangle(Vector3 tr0, Vector3 tr1, Vector3 tr2, Vector3 point) { Vector3 edge0 = tr1 - tr0; Vector3 edge1 = tr2 - tr0; Vector3 v0 = tr0 - point; float a = Vector3.Dot(edge0, edge0); //edge0.dot( edge0 ); //Equals to sqrMagnitude float b = Vector3.Dot(edge0, edge1); float c = Vector3.Dot(edge1, edge1); //Equals to sqrMagnitude float d = Vector3.Dot(edge0, v0); float e = Vector3.Dot(edge1, v0); float det = a * c - b * b; float s = b * e - c * d; float t = b * d - a * e; if (s + t < det) { if (s < 0.0F) { if (t < 0.0F) { if (d < 0.0F) { s = Mathfx.Clamp01(-d / a); t = 0.0F; } else { s = 0.0F; t = Mathfx.Clamp01(-e / c); } } else { s = 0.0F; t = Mathfx.Clamp01(-e / c); } } else if (t < 0.0F) { s = Mathfx.Clamp01(-d / a); t = 0.0F; } else { float invDet = 1.0F / det; s *= invDet; t *= invDet; } } else { if (s < 0.0F) { float tmp0 = b + d; float tmp1 = c + e; if (tmp1 > tmp0) { float numer = tmp1 - tmp0; float denom = a - 2 * b + c; s = Mathfx.Clamp01(numer / denom); t = 1 - s; } else { t = Mathfx.Clamp01(-e / c); s = 0.0F; } } else if (t < 0.0F) { if (a + d > b + e) { float numer = c + e - b - d; float denom = a - 2 * b + c; s = Mathfx.Clamp01(numer / denom); t = 1 - s; } else { s = Mathfx.Clamp01(-e / c); t = 0.0F; } } else { float numer = c + e - b - d; float denom = a - 2 * b + c; s = Mathfx.Clamp01(numer / denom); t = 1.0F - s; } } return(tr0 + s * edge0 + t * edge1); }
/* F score. The F score is the #g score + #h score, that is the cost it taken to move to this node from the start + the estimated cost to move to the end node.\n * Nodes are sorted by their F score, nodes with lower F scores are opened first */ /*public uint f { * get { * return g+h; * } * }*/ /** Calculates and updates the H score. * Calculates the H score with respect to the target position and chosen heuristic. * \param targetPosition The position to calculate the distance to. * \param heuristic Heuristic to use. The heuristic can also be hard coded using pre processor directives (check sourcecode) * \param scale Scale of the heuristic * \param nodeR NodeRun object associated with this node. */ public void UpdateH(Int3 targetPosition, Heuristic heuristic, float scale, NodeRun nodeR) { //Choose the correct heuristic, compute it and store it in the \a h variable if (heuristic == Heuristic.None) { nodeR.h = 0; return; } #if AstarFree || !DefinedHeuristic if (heuristic == Heuristic.Euclidean) { nodeR.h = (uint)Mathfx.RoundToInt((position - targetPosition).magnitude * scale); } else if (heuristic == Heuristic.Manhattan) { nodeR.h = (uint)Mathfx.RoundToInt( (Abs(position.x - targetPosition.x) + Abs(position.y - targetPosition.y) + Abs(position.z - targetPosition.z)) * scale ); } else //if (heuristic == Heuristic.DiagonalManhattan) { { int xDistance = Abs(position.x - targetPosition.x); int zDistance = Abs(position.z - targetPosition.z); if (xDistance > zDistance) { nodeR.h = (uint)(14 * zDistance + 10 * (xDistance - zDistance)) / 10; } else { nodeR.h = (uint)(14 * xDistance + 10 * (zDistance - xDistance)) / 10; } nodeR.h = (uint)Mathfx.RoundToInt(nodeR.h * scale); } #elif NoHeuristic nodeR.h = 0; #elif ManhattanHeuristic nodeR.h = (uint)Mathfx.RoundToInt( (Abs(position.x - targetPosition.x) + Abs(position.y - targetPosition.y) + Abs(position.z - targetPosition.z)) * scale ); #elif DiagonalManhattanHeuristic int xDistance = Abs(position.x - targetPosition.x); int zDistance = Abs(position.z - targetPosition.z); if (xDistance > zDistance) { nodeR.h = (uint)(14 * zDistance + 10 * (xDistance - zDistance)) / 10; } else { nodeR.h = (uint)(14 * xDistance + 10 * (zDistance - xDistance)) / 10; } nodeR.h = (uint)Mathfx.RoundToInt(nodeR.h * scale); #elif EucledianHeuristic nodeR.h = (uint)Mathfx.RoundToInt((position - targetPosition).magnitude * scale); #endif }
/** 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); }