/** 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);
}
