/** Returns the closest point of the node */
public static Vector3 ClosestPointOnNode(MeshNode node, Int3[] vertices, Vector3 pos)
{
return(Polygon.ClosesPointOnTriangle(vertices[node[0]], vertices[node[1]], vertices[node[2]], pos));
}
public Vector3 ClosestPoint(Vector3 p, Int3[] vertices)
{
return(Polygon.ClosesPointOnTriangle(vertices[v1], vertices[v2], vertices[v3], p));
}
/** 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.ClosesPointOnTriangle((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.ClosesPointOnTriangle((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.ClosesPointOnTriangle((Vector3)vertices[node.v1], (Vector3)vertices[node.v2], (Vector3)vertices[node.v3], position);
nninfo.constClampedPosition = clP;
}
return(nninfo);
}
