/** Returns if there is an obstacle between \a \a and \a \b on the graph.
* This function is different from the other Linecast functions since it 1) snaps the start and end positions directly to the graph.
*
* \param [in] a Point to linecast from
* \param [in] b Point to linecast to
* \param [out] hit Contains info on what was hit, see GraphHitInfo
* \param [in] hint \deprecated
*
* This is not the same as Physics.Linecast, this function traverses the graph and looks for collisions.
*
* \version Since 3.6.8 this method uses the same implementation as the other linecast methods so there is no performance boost to using it.
* \version In 3.6.8 this method was rewritten and that fixed a large number of bugs.
* Previously it had not always followed the line exactly as it should have
* and the hit output was not very accurate
* (for example the hit point was just the node position instead of a point on the edge which was hit)
*/
public bool SnappedLinecast (Vector3 a, Vector3 b, GraphNode hint, out GraphHitInfo hit) {
return Linecast (
(Vector3)GetNearest (a,NNConstraint.None).node.position,
(Vector3)GetNearest (b,NNConstraint.None).node.position,
hint,
out hit
);
}
/** Returns if there is an obstacle between \a origin and \a end on the graph.
* \param [in] _a Point to linecast from
* \param [in] _b Point to linecast to
* \param [out] hit Contains info on what was hit, see GraphHitInfo
* \param [in] hint If you have some idea of what the start node might be (the one close to \a _a), pass it to hint since it can enable faster lookups
* This is not the same as Physics.Linecast, this function traverses the graph and looks for collisions.
* \astarpro */
public bool Linecast (Vector3 _a, Vector3 _b, GraphNode hint, out GraphHitInfo hit) {
return Linecast (_a, _b, hint, out hit, null);
}
/** Returns if there is an obstacle between \a _a and \a _b on the graph.
* \param [in] _a Point to linecast from
* \param [in] _b Point to linecast to
* \param [out] hit Contains info on what was hit, see GraphHitInfo
* \param [in] hint \deprecated
* \param trace If a list is passed, then it will be filled with all nodes the linecast traverses
*
* This is not the same as Physics.Linecast, this function traverses the graph and looks for collisions.
*
* It uses a method similar to Bresenham's line algorithm but it has been
* extended to allow the start and end points to lie on non-integer coordinates
* (which makes the math a bit trickier).
*
* \see https://en.wikipedia.org/wiki/Bresenham's_line_algorithm
*
* \version In 3.6.8 this method was rewritten to improve accuracy and performance.
* Previously it used a sampling approach which could cut corners of obstacles slightly
* and was pretty inefficient.
*
* \astarpro
*/
public bool Linecast (Vector3 _a, Vector3 _b, GraphNode hint, out GraphHitInfo hit, List<GraphNode> trace) {
hit = new GraphHitInfo ();
hit.origin = _a;
Vector3 aInGraphSpace = inverseMatrix.MultiplyPoint3x4(_a);
Vector3 bInGraphSpace = inverseMatrix.MultiplyPoint3x4(_b);
// Clip the line so that the start and end points are on the graph
if (!ClipLineSegmentToBounds (aInGraphSpace, bInGraphSpace, out aInGraphSpace, out bInGraphSpace)) {
// Line does not intersect the graph
// So there are no obstacles we can hit
return false;
}
// Find the closest nodes to the start and end on the part of the segment which is on the graph
var n1 = GetNearest (matrix.MultiplyPoint3x4(aInGraphSpace),NNConstraint.None).node as GridNodeBase;
var n2 = GetNearest (matrix.MultiplyPoint3x4(bInGraphSpace),NNConstraint.None).node as GridNodeBase;
if (!n1.Walkable) {
hit.node = n1;
// Hit point is the point where the segment intersects with the graph boundary
// or just _a if it starts inside the graph
hit.point = matrix.MultiplyPoint3x4(aInGraphSpace);
hit.tangentOrigin = hit.point;
return true;
}
// Throw away components we don't care about (y)
var a = new Vector2(aInGraphSpace.x,aInGraphSpace.z);
var b = new Vector2(bInGraphSpace.x,bInGraphSpace.z);
// Subtract 0.5 because nodes have an offset of 0.5 (first node is at (0.5,0.5) not at (0,0))
// And it's just more convenient to remove that term here
a -= Vector2.one*0.5f;
b -= Vector2.one*0.5f;
// Couldn't find a valid node
// This shouldn't really happen unless there are NO nodes in the graph
if (n1 == null || n2 == null) {
hit.node = null;
hit.point = _a;
return true;
}
var dir = b-a;
// Primary direction that we will move in
// (e.g up and right or down and left)
var sign = new Int2((int)Mathf.Sign(dir.x), (int)Mathf.Sign(dir.y));
// How much further we move away from (or towards) the line when walking along #sign
// This isn't an actual distance. It is a signed distance so it can be negative (other side of the line)
// Also it includes an additional factor, but the same factor is used everywhere
// and we only check for if the signed distance is greater or equal to zero so it is ok
var primaryDirectionError = CrossMagnitude(dir, new Vector2(sign.x,sign.y))*0.5f;
/* Z
* |
* |
*
* 2
* |
* -- 3 - X - 1 ----- X
* |
* 0
*
* |
* |
*/
// This is the direction which moves further to the right of the segment (when looking from the start)
int directionToReduceError;
// This is the direction which moves further to the left of the segment (when looking from the start)
int directionToIncreaseError;
if (dir.y >= 0) {
if (dir.x >= 0) {
// First quadrant
directionToReduceError = 1;
directionToIncreaseError = 2;
} else {
// Second quadrant
directionToReduceError = 2;
directionToIncreaseError = 3;
}
} else {
if (dir.x < 0) {
// Third quadrant
directionToReduceError = 3;
directionToIncreaseError = 0;
} else {
// Fourth quadrant
directionToReduceError = 0;
directionToIncreaseError = 1;
}
}
// Current node. Start at n1
var current = n1;
while (current.NodeInGridIndex != n2.NodeInGridIndex) {
// We visited #current so add it to the trace
if (trace != null) {
trace.Add (current);
}
// Position of the node in 2D graph/node space
// Here the first node in the graph is at (0,0)
var p = new Vector2(current.NodeInGridIndex % width, current.NodeInGridIndex / width);
// Calculate the error
// This is proportional to the distance between the line and the node
var error = CrossMagnitude(dir, p-a);
// How does the error change we take one step in the primary direction
var nerror = error + primaryDirectionError;
// Check if we need to reduce or increase the error (we want to keep it near zero)
// and pick the appropriate direction to move in
int ndir = nerror < 0 ? directionToIncreaseError : directionToReduceError;
// Check we can move in that direction
var other = GetNeighbourAlongDirection(current, ndir);
if (other != null) {
current = other;
} else {
// Hit obstacle
// We know from what direction we moved in
// so we can calculate the line which we hit
// Either X offset is 0 or Z offset is zero since we only move in one of the 4 axis aligned directions
// The line we hit will be right between two nodes (so a distance of 0.5 from the current node in graph space)
Vector2 lineOrigin = p + new Vector2 (neighbourXOffsets[ndir], neighbourZOffsets[ndir]) * 0.5f;
Vector2 lineDirection;
if (neighbourXOffsets[ndir] == 0) {
// We hit a line parallel to the X axis
lineDirection = new Vector2(1,0);
} else {
// We hit a line parallel to the Z axis
lineDirection = new Vector2(0,1);
}
// Find the intersection
var intersection = Polygon.IntersectionPoint (lineOrigin, lineOrigin+lineDirection, a, b);
var currentNodePositionInGraphSpace = inverseMatrix.MultiplyPoint3x4((Vector3)current.position);
// The intersection is in graph space (with an offset of 0.5) so we need to transform it to world space
var intersection3D = new Vector3(intersection.x + 0.5f, currentNodePositionInGraphSpace.y, intersection.y + 0.5f);
var lineOrigin3D = new Vector3(lineOrigin.x + 0.5f, currentNodePositionInGraphSpace.y, lineOrigin.y + 0.5f);
hit.point = matrix.MultiplyPoint3x4(intersection3D);
hit.tangentOrigin = matrix.MultiplyPoint3x4(lineOrigin3D);
hit.tangent = matrix.MultiplyVector(new Vector3(lineDirection.x,0,lineDirection.y));
hit.node = current;
return true;
}
}
// Enqueue the last node to the trace
if (trace != null) {
trace.Add (current);
}
// No obstacles detected
if (current == n2) {
return false;
}
// Reached node right above or right below n2 but we cannot reach it
hit.point = (Vector3)current.position;
hit.tangentOrigin = hit.point;
return true;
}
/** Returns if there is an obstacle between \a origin and \a end on the graph.
* \param [in] tmp_origin Point to start from
* \param [in] tmp_end Point to linecast to
* \param [out] hit Contains info on what was hit, see GraphHitInfo
* \param [in] hint You need to pass the node closest to the start point, if null, a search for the closest node will be done
* \param trace If a list is passed, then it will be filled with all nodes the linecast traverses
* This is not the same as Physics.Linecast, this function traverses the \b graph and looks for collisions instead of checking for collider intersection.
* \astarpro */
public bool Linecast (Vector3 tmp_origin, Vector3 tmp_end, GraphNode hint, out GraphHitInfo hit, List<GraphNode> trace) {
return NavMeshGraph.Linecast (this, tmp_origin, tmp_end, hint, out hit, trace);
}
public bool Linecast (Vector3 origin, Vector3 end, GraphNode hint, out GraphHitInfo hit) {
return NavMeshGraph.Linecast (this as INavmesh, origin,end,hint, out hit, null);
}
/** Returns if there is an obstacle between \a origin and \a end on the graph.
* \param [in] graph The graph to perform the search on
* \param [in] tmp_origin Point to start from
* \param [in] tmp_end Point to linecast to
* \param [out] hit Contains info on what was hit, see GraphHitInfo
* \param [in] hint You need to pass the node closest to the start point, if null, a search for the closest node will be done
* \param trace If a list is passed, then it will be filled with all nodes the linecast traverses
* This is not the same as Physics.Linecast, this function traverses the \b graph and looks for collisions instead of checking for collider intersection.
* \astarpro */
public static bool Linecast (INavmesh graph, Vector3 tmp_origin, Vector3 tmp_end, GraphNode hint, out GraphHitInfo hit, List<GraphNode> trace) {
var end = (Int3)tmp_end;
var origin = (Int3)tmp_origin;
hit = new GraphHitInfo ();
if (float.IsNaN (tmp_origin.x + tmp_origin.y + tmp_origin.z)) throw new System.ArgumentException ("origin is NaN");
if (float.IsNaN (tmp_end.x + tmp_end.y + tmp_end.z)) throw new System.ArgumentException ("end is NaN");
var node = hint as TriangleMeshNode;
if (node == null) {
node = (graph as NavGraph).GetNearest (tmp_origin, NNConstraint.None).node as TriangleMeshNode;
if (node == null) {
Debug.LogError ("Could not find a valid node to start from");
hit.point = tmp_origin;
return true;
}
}
if (origin == end) {
hit.node = node;
return false;
}
origin = (Int3)node.ClosestPointOnNode ((Vector3)origin);
hit.origin = (Vector3)origin;
if (!node.Walkable) {
hit.point = (Vector3)origin;
hit.tangentOrigin = (Vector3)origin;
return true;
}
List<Vector3> left = ListPool<Vector3>.Claim();//new List<Vector3>(1);
List<Vector3> right = ListPool<Vector3>.Claim();//new List<Vector3>(1);
int counter = 0;
while (true) {
counter++;
if ( counter > 2000 ) {
Debug.LogError ("Linecast was stuck in infinite loop. Breaking.");
ListPool<Vector3>.Release(left);
ListPool<Vector3>.Release(right);
return true;
}
TriangleMeshNode newNode = null;
if (trace != null) trace.Add (node);
if (node.ContainsPoint (end)) {
ListPool<Vector3>.Release(left);
ListPool<Vector3>.Release(right);
return false;
}
for (int i=0;i<node.connections.Length;i++) {
//Nodes on other graphs should not be considered
//They might even be of other types (not MeshNode)
if (node.connections[i].GraphIndex != node.GraphIndex) continue;
left.Clear();
right.Clear();
if (!node.GetPortal (node.connections[i],left,right,false)) continue;
Vector3 a = left[0];
Vector3 b = right[0];
//i.e Right or colinear
if (!Polygon.LeftNotColinear (a,b,hit.origin)) {
if (Polygon.LeftNotColinear (a, b, tmp_end)) {
//Since polygons are laid out in clockwise order, the ray would intersect (if intersecting) this edge going in to the node, not going out from it
continue;
}
}
float factor1, factor2;
if (Polygon.IntersectionFactor (a,b,hit.origin,tmp_end, out factor1, out factor2)) {
//Intersection behind the start
if (factor2 < 0) continue;
if (factor1 >= 0 && factor1 <= 1) {
newNode = node.connections[i] as TriangleMeshNode;
break;
}
}
}
if (newNode == null) {
//Possible edge hit
int vs = node.GetVertexCount();
for (int i=0;i<vs;i++) {
var a = (Vector3)node.GetVertex(i);
var b = (Vector3)node.GetVertex((i + 1) % vs);
//i.e right or colinear
if (!Polygon.LeftNotColinear (a,b,hit.origin)) {
//Since polygons are laid out in clockwise order, the ray would intersect (if intersecting) this edge going in to the node, not going out from it
if (Polygon.LeftNotColinear (a, b, tmp_end)) {
//Since polygons are laid out in clockwise order, the ray would intersect (if intersecting) this edge going in to the node, not going out from it
continue;
}
}
float factor1, factor2;
if (Polygon.IntersectionFactor (a,b,hit.origin,tmp_end, out factor1, out factor2)) {
if (factor2 < 0) continue;
if (factor1 >= 0 && factor1 <= 1) {
Vector3 intersectionPoint = a + (b-a)*factor1;
hit.point = intersectionPoint;
hit.node = node;
hit.tangent = b-a;
hit.tangentOrigin = a;
ListPool<Vector3>.Release(left);
ListPool<Vector3>.Release(right);
return true;
}
}
}
//Ok, this is wrong...
Debug.LogWarning ("Linecast failing because point not inside node, and line does not hit any edges of it");
ListPool<Vector3>.Release(left);
ListPool<Vector3>.Release(right);
return false;
}
node = newNode;
}
}
/** Returns if there is an obstacle between \a origin and \a end on the graph.
* \param [in] graph The graph to perform the search on
* \param [in] tmp_origin Point to start from
* \param [in] tmp_end Point to linecast to
* \param [out] hit Contains info on what was hit, see GraphHitInfo
* \param [in] hint You need to pass the node closest to the start point, if null, a search for the closest node will be done
* This is not the same as Physics.Linecast, this function traverses the \b graph and looks for collisions instead of checking for collider intersection.
* \astarpro */
public static bool Linecast (INavmesh graph, Vector3 tmp_origin, Vector3 tmp_end, GraphNode hint, out GraphHitInfo hit) {
return Linecast (graph, tmp_origin, tmp_end, hint, out hit, null);
}
/** Returns if there is an obstacle between \a origin and \a end on the graph.
* \param [in] origin Point to linecast from
* \param [in] end Point to linecast to
* \param [out] hit Contains info on what was hit, see GraphHitInfo
* \param [in] hint You need to pass the node closest to the start point
* \param trace If a list is passed, then it will be filled with all nodes the linecast traverses
* This is not the same as Physics.Linecast, this function traverses the \b graph and looks for collisions instead of checking for collider intersection.
* \astarpro */
public bool Linecast (Vector3 origin, Vector3 end, GraphNode hint, out GraphHitInfo hit, List<GraphNode> trace) {
return Linecast (this as INavmesh, origin,end,hint, out hit, trace);
}
