public bool FindNearestNonBlocked( Vector2 fromPoint, out Vector2 goTo )
{
goTo = Vector2.Zero;
if( _spatialGraph == null )
return false;
SpatialGraphKDNode currentNode = _spatialGraph.FindNode( ref fromPoint );
if( currentNode == null )
return false;
float fMinDistance = float.MaxValue;
SpatialGraphKDNode nearestNeighbor = null;
//otherwise, iterate over neighbors to find a non-blocked
for( int i = 0; i < currentNode.Neighbors.Count; i++ )
{
SpatialGraphKDNode neighbor = currentNode.Neighbors[i];
if( neighbor.Blocked )
continue;
Vector2 vDir = neighbor.BBox.Centroid() - fromPoint;
Ray2D ray = new Ray2D( fromPoint, Vector2.Normalize(vDir) );
float distanceToBBox;
if( neighbor.BBox.Intersects(ref ray, out distanceToBBox) )
{
if( distanceToBBox < fMinDistance )
{
fMinDistance = distanceToBBox;
nearestNeighbor = neighbor;
}
}
}
if (nearestNeighbor != null)
{
goTo = nearestNeighbor.BBox.Centroid();
return true;
}
return false;
}
public bool Intersects(ref Ray2D ray, out float distanceAlongRay)
{
distanceAlongRay = 0.0f;
float maxValue = float.MaxValue;
if (Math.Abs(ray.Direction.X) < MathUtil.Epsilon)
{
if ((ray.Position.X < Min.X) || (ray.Position.X > Max.X))
{
return false;
}
}
else
{
float invMag = 1.0f / ray.Direction.X;
float minProj = (Min.X - ray.Position.X) * invMag;
float maxProj = (Max.X - ray.Position.X) * invMag;
if (minProj > maxProj)
{
float temp = minProj;
minProj = maxProj;
maxProj = temp;
}
distanceAlongRay = Math.Max(minProj, distanceAlongRay);
maxValue = Math.Min(maxProj, maxValue);
if (distanceAlongRay > maxValue)
{
return false;
}
}
if (Math.Abs(ray.Direction.Y) < MathUtil.Epsilon)
{
if ((ray.Position.Y < Min.Y) || (ray.Position.Y > Max.Y))
{
return false;
}
}
else
{
float invMag = 1.0f / ray.Direction.Y;
float minProj = (Min.Y - ray.Position.Y) * invMag;
float maxProj = (Max.Y - ray.Position.Y) * invMag;
if (minProj > maxProj)
{
float temp = minProj;
minProj = maxProj;
maxProj = temp;
}
distanceAlongRay = Math.Max(minProj, distanceAlongRay);
maxValue = Math.Min(maxProj, maxValue);
if (distanceAlongRay > maxValue)
{
return false;
}
}
return true;
}