/// <summary>
/// 判断两点之间是否有障碍，并返回碰撞点信息
/// </summary>
/// <param name="startPoint"></param>
/// <param name="endPoint"></param>
/// <param name="hit"></param>
/// <returns></returns>
public static bool isHit(VInt3 startPoint, VInt3 endPoint, out GraphHitInfo hit)
{
GraphNode startNode = AstarPath.active.GetNearest(startPoint).node;
var graph = AstarData.GetGraph(startNode) as NavmeshBase;
return(graph.Linecast(startPoint, endPoint, startNode, out hit));
}
/** 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
* 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(Int3 origin, Int3 end, GraphNode hint, out GraphHitInfo hit)
{
return(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] _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
* \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.
* \astarpro */
public bool Linecast (Vector3 _a, Vector3 _b, GraphNode hint, out GraphHitInfo hit, List<GraphNode> trace) {
hit = new GraphHitInfo ();
//
//Node n2 = GetNearest (_b,NNConstraint.None);
_a = inverseMatrix.MultiplyPoint3x4 (_a);
_a.x -= 0.5F;
_a.z -= 0.5F;
_b = inverseMatrix.MultiplyPoint3x4 (_b);
_b.x -= 0.5F;
_b.z -= 0.5F;
//Grid coordinates
//Int3 a = new Int3 (Mathf.RoundToInt (_a.x),Mathf.RoundToInt (_a.y),Mathf.RoundToInt (_a.z));
//Int3 b = new Int3 (Mathf.RoundToInt (_b.x),Mathf.RoundToInt (_b.y),Mathf.RoundToInt (_b.z));
//Clamping is needed
if (_a.x < -0.5F || _a.z < -0.5F || _a.x >= width-0.5F || _a.z >= depth-0.5F ||
_b.x < -0.5F || _b.z < -0.5F || _b.x >= width-0.5F || _b.z >= depth-0.5F) {
//Bounding points of the grid
Vector3 p1 = new Vector3 (-0.5F ,0, -0.5F);
Vector3 p2 = new Vector3 (-0.5F ,0, depth-0.5F);
Vector3 p3 = new Vector3 (width-0.5F,0, depth-0.5F);
Vector3 p4 = new Vector3 (width-0.5F,0, -0.5F);
int intersectCount = 0;
bool intersect = false;
Vector3 intersection = Polygon.SegmentIntersectionPoint (p1,p2,_a,_b, out intersect);
if (intersect) {
//Debug.Log ("Intersection with p1 and p2 "+_a+" "+_b+" - Intersection: "+intersection);
intersectCount++;
if (!Polygon.Left (p1,p2,_a)) {
_a = intersection;
} else {
_b = intersection;
}
}
intersection = Polygon.SegmentIntersectionPoint (p2,p3,_a,_b, out intersect);
if (intersect) {
//Debug.Log ("Intersection with p2 and p3 "+_a+" "+_b+" - Intersection: "+intersection);
intersectCount++;
if (!Polygon.Left (p2,p3,_a)) {
_a = intersection;
} else {
_b = intersection;
}
}
intersection = Polygon.SegmentIntersectionPoint (p3,p4,_a,_b, out intersect);
if (intersect) {
//Debug.Log ("Intersection with p3 and p4 "+_a+" "+_b+" - Intersection: "+intersection);
intersectCount++;
if (!Polygon.Left (p3,p4,_a)) {
_a = intersection;
} else {
_b = intersection;
}
}
intersection = Polygon.SegmentIntersectionPoint (p4,p1,_a,_b, out intersect);
if (intersect) {
//Debug.Log ("Intersection with p4 and p1 "+_a+" "+_b+" - Intersection: "+intersection);
intersectCount++;
if (!Polygon.Left (p4,p1,_a)) {
_a = intersection;
} else {
_b = intersection;
}
}
if (intersectCount == 0) {
//The line does not intersect with the grid
return false;
}
}
Vector3 dir = _b-_a;
float magn = dir.magnitude;
if (magn == 0) {
//Zero length line
return false;
}
float sampleLength = 0.2F;
float newMagn = nodeSize * sampleLength;
newMagn -= nodeSize * 0.02F;
dir = (dir / magn) * newMagn;
//Floor to int, number of samples on the line
int its = (int)(magn / newMagn);
//Debug.Log ("Num Its: "+its+" "+dir);
Vector3 originOffset = _a + dir * nodeSize * 0.01F;
GraphNode prevNode = null;
for (int i=0;i <= its;i++) {
Vector3 p = originOffset + dir * i;
int x = Mathf.RoundToInt (p.x);
int z = Mathf.RoundToInt (p.z);
x = x < 0 ? 0 : (x >= width ? width-1 : x);
z = z < 0 ? 0 : (z >= depth ? depth-1 : z);
/*if (x < 0 || z < 0 || x >= width || z >= depth) {
Debug.LogError ("Point Out Of Bounds "+"("+x+", "+z+") "+p+" in iteration "+i+" of "+its+" With a direction magn "+dir.magnitude+"\nA: "+_a+"\nB: "+_b);
throw new System.IndexOutOfRangeException ("The point "+x+","+z+ " is outside the bounds of the grid");
break;
}*/
GraphNode node = nodes[z*width+x];
if (node == prevNode) continue;
if (!node.Walkable) {
if (i > 0) {
hit.point = matrix.MultiplyPoint3x4 (originOffset + dir * (i-1)+new Vector3 (0.5F,0,0.5F));
} else {
hit.point = matrix.MultiplyPoint3x4 (_a+new Vector3 (0.5F,0,0.5F));
}
hit.origin = matrix.MultiplyPoint3x4 (_a+new Vector3 (0.5F,0,0.5F));
hit.node = node;
return true;
}
if (i > its-1) {
if (Mathf.Abs (p.x-_b.x) <= 0.50001F || Mathf.Abs (p.z - _b.z) <= 0.50001F) {
return false;
}
}
if (trace != null) trace.Add (node);
prevNode = node;
}
return false;
}
/** Returns if \a _b is visible from \a _a 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 new bool Linecast(Vector3 _a, Vector3 _b, Node hint, out GraphHitInfo hit)
{
return SnappedLinecast (_a,_b,hint,out hit);
}
public static bool Linecast(INavmesh graph, Vector3 tmp_origin, Vector3 tmp_end, GraphNode hint, out GraphHitInfo hit, List <GraphNode> trace)
{
Int3 @int = (Int3)tmp_end;
Int3 int2 = (Int3)tmp_origin;
hit = default(GraphHitInfo);
if (float.IsNaN(tmp_origin.x + tmp_origin.y + tmp_origin.z))
{
throw new ArgumentException("origin is NaN");
}
if (float.IsNaN(tmp_end.x + tmp_end.y + tmp_end.z))
{
throw new ArgumentException("end is NaN");
}
TriangleMeshNode triangleMeshNode = hint as TriangleMeshNode;
if (triangleMeshNode == null)
{
triangleMeshNode = ((graph as NavGraph).GetNearest(tmp_origin, NNConstraint.None).node as TriangleMeshNode);
if (triangleMeshNode == null)
{
Debug.LogError("Could not find a valid node to start from");
hit.point = tmp_origin;
return(true);
}
}
if (int2 == @int)
{
hit.node = triangleMeshNode;
return(false);
}
int2 = (Int3)triangleMeshNode.ClosestPointOnNode((Vector3)int2);
hit.origin = (Vector3)int2;
if (!triangleMeshNode.Walkable)
{
hit.point = (Vector3)int2;
hit.tangentOrigin = (Vector3)int2;
return(true);
}
List <Vector3> list = ListPool <Vector3> .Claim();
List <Vector3> list2 = ListPool <Vector3> .Claim();
int num = 0;
while (true)
{
num++;
if (num > 2000)
{
break;
}
TriangleMeshNode triangleMeshNode2 = null;
if (trace != null)
{
trace.Add(triangleMeshNode);
}
if (triangleMeshNode.ContainsPoint(@int))
{
goto Block_9;
}
for (int i = 0; i < triangleMeshNode.connections.Length; i++)
{
if (triangleMeshNode.connections[i].GraphIndex == triangleMeshNode.GraphIndex)
{
list.Clear();
list2.Clear();
if (triangleMeshNode.GetPortal(triangleMeshNode.connections[i], list, list2, false))
{
Vector3 vector = list[0];
Vector3 vector2 = list2[0];
if (Polygon.LeftNotColinear(vector, vector2, hit.origin) || !Polygon.LeftNotColinear(vector, vector2, tmp_end))
{
float num2;
float num3;
if (Polygon.IntersectionFactor(vector, vector2, hit.origin, tmp_end, out num2, out num3))
{
if (num3 >= 0f)
{
if (num2 >= 0f && num2 <= 1f)
{
triangleMeshNode2 = (triangleMeshNode.connections[i] as TriangleMeshNode);
break;
}
}
}
}
}
}
}
if (triangleMeshNode2 == null)
{
goto Block_18;
}
triangleMeshNode = triangleMeshNode2;
}
Debug.LogError("Linecast was stuck in infinite loop. Breaking.");
ListPool <Vector3> .Release(list);
ListPool <Vector3> .Release(list2);
return(true);
Block_9:
ListPool <Vector3> .Release(list);
ListPool <Vector3> .Release(list2);
return(false);
Block_18:
int vertexCount = triangleMeshNode.GetVertexCount();
for (int j = 0; j < vertexCount; j++)
{
Vector3 vector3 = (Vector3)triangleMeshNode.GetVertex(j);
Vector3 vector4 = (Vector3)triangleMeshNode.GetVertex((j + 1) % vertexCount);
if (Polygon.LeftNotColinear(vector3, vector4, hit.origin) || !Polygon.LeftNotColinear(vector3, vector4, tmp_end))
{
float num4;
float num5;
if (Polygon.IntersectionFactor(vector3, vector4, hit.origin, tmp_end, out num4, out num5))
{
if (num5 >= 0f)
{
if (num4 >= 0f && num4 <= 1f)
{
Vector3 point = vector3 + (vector4 - vector3) * num4;
hit.point = point;
hit.node = triangleMeshNode;
hit.tangent = vector4 - vector3;
hit.tangentOrigin = vector3;
ListPool <Vector3> .Release(list);
ListPool <Vector3> .Release(list2);
return(true);
}
}
}
}
}
Debug.LogWarning("Linecast failing because point not inside node, and line does not hit any edges of it");
ListPool <Vector3> .Release(list);
ListPool <Vector3> .Release(list2);
return(false);
}
public bool Linecast(Vector3 origin, Vector3 end, GraphNode hint, out GraphHitInfo hit, List <GraphNode> trace)
{
return(NavMeshGraph.Linecast(this, origin, end, hint, out hit, trace));
}
public new bool SnappedLinecast(Vector3 _a, Vector3 _b, GraphNode hint, out GraphHitInfo hit)
{
hit = default(GraphHitInfo);
LevelGridNode levelGridNode = base.GetNearest(_a, NNConstraint.None).node as LevelGridNode;
LevelGridNode levelGridNode2 = base.GetNearest(_b, NNConstraint.None).node as LevelGridNode;
if (levelGridNode == null || levelGridNode2 == null)
{
hit.node = null;
hit.point = _a;
return(true);
}
_a = this.inverseMatrix.MultiplyPoint3x4((Vector3)levelGridNode.position);
_a.x -= 0.5f;
_a.z -= 0.5f;
_b = this.inverseMatrix.MultiplyPoint3x4((Vector3)levelGridNode2.position);
_b.x -= 0.5f;
_b.z -= 0.5f;
Int3 ob = new Int3(Mathf.RoundToInt(_a.x), Mathf.RoundToInt(_a.y), Mathf.RoundToInt(_a.z));
Int3 @int = new Int3(Mathf.RoundToInt(_b.x), Mathf.RoundToInt(_b.y), Mathf.RoundToInt(_b.z));
hit.origin = (Vector3)ob;
if (!levelGridNode.Walkable)
{
hit.node = levelGridNode;
hit.point = this.matrix.MultiplyPoint3x4(new Vector3((float)ob.x + 0.5f, 0f, (float)ob.z + 0.5f));
hit.point.y = ((Vector3)hit.node.position).y;
return(true);
}
int num = Mathf.Abs(ob.x - @int.x);
int num2 = Mathf.Abs(ob.z - @int.z);
LevelGridNode levelGridNode4;
for (LevelGridNode levelGridNode3 = levelGridNode; levelGridNode3 != levelGridNode2; levelGridNode3 = levelGridNode4)
{
if (levelGridNode3.NodeInGridIndex == levelGridNode2.NodeInGridIndex)
{
hit.node = levelGridNode3;
hit.point = (Vector3)levelGridNode3.position;
return(true);
}
num = Math.Abs(ob.x - @int.x);
num2 = Math.Abs(ob.z - @int.z);
int num3 = 0;
if (num >= num2)
{
num3 = ((@int.x <= ob.x) ? 3 : 1);
}
else if (num2 > num)
{
num3 = ((@int.z <= ob.z) ? 0 : 2);
}
if (!this.CheckConnection(levelGridNode3, num3))
{
hit.node = levelGridNode3;
hit.point = (Vector3)levelGridNode3.position;
return(true);
}
levelGridNode4 = this.nodes[levelGridNode3.NodeInGridIndex + this.neighbourOffsets[num3] + this.width * this.depth * levelGridNode3.GetConnectionValue(num3)];
if (!levelGridNode4.Walkable)
{
hit.node = levelGridNode4;
hit.point = (Vector3)levelGridNode4.position;
return(true);
}
ob = (Int3)this.inverseMatrix.MultiplyPoint3x4((Vector3)levelGridNode4.position);
}
return(false);
}
/** 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) {
Int3 end = (Int3)tmp_end;
Int3 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");
TriangleMeshNode 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 = Pathfinding.Util.ListPool<Vector3>.Claim();//new List<Vector3>(1);
List<Vector3> right = Pathfinding.Util.ListPool<Vector3>.Claim();//new List<Vector3>(1);
while (true) {
TriangleMeshNode newNode = null;
if (trace != null) trace.Add (node);
if (node.ContainsPoint (end)) {
Pathfinding.Util.ListPool<Vector3>.Release(left);
Pathfinding.Util.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++) {
Vector3 a = (Vector3)node.GetVertex(i);
Vector3 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;
Pathfinding.Util.ListPool<Vector3>.Release(left);
Pathfinding.Util.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");
Pathfinding.Util.ListPool<Vector3>.Release(left);
Pathfinding.Util.ListPool<Vector3>.Release(right);
return false;
}
node = newNode;
}
}
/** 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;
}
}
// Add 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;
}
public bool Linecast (Vector3 origin, Vector3 end, Node hint, out GraphHitInfo hit) {
return NavMeshGraph.Linecast (this as INavmesh, origin,end,hint,false,0, out hit);
}
public bool Linecast(Vector3 origin, Vector3 end, Node hint, out GraphHitInfo hit)
{
return(NavMeshGraph.Linecast(this as INavmesh, origin, end, hint, false, 0, out hit));
}
/** 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, Int3 tmp_origin, Int3 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(Int3 origin, Int3 end, GraphNode hint, out GraphHitInfo hit, List <GraphNode> trace)
{
return(Linecast(this as INavmesh, origin, end, hint, out hit, trace));
}
/** 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);
}
/** 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] 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);
}
public static bool Linecast(INavmesh graph, VInt3 tmp_origin, VInt3 tmp_end, GraphNode hint, out GraphHitInfo hit, List<GraphNode> trace)
{
VInt3 vInt = tmp_end;
VInt3 vInt2 = tmp_origin;
hit = default(GraphHitInfo);
if (float.IsNaN((float)(tmp_origin.x + tmp_origin.y + tmp_origin.z)))
{
throw new ArgumentException("origin is NaN");
}
if (float.IsNaN((float)(tmp_end.x + tmp_end.y + tmp_end.z)))
{
throw new ArgumentException("end is NaN");
}
TriangleMeshNode triangleMeshNode = hint as TriangleMeshNode;
if (triangleMeshNode == null)
{
triangleMeshNode = ((graph as NavGraph).GetNearest(tmp_origin, NNConstraint.None).node as TriangleMeshNode);
if (triangleMeshNode == null)
{
Debug.LogError("Could not find a valid node to start from");
hit.point = tmp_origin;
return true;
}
}
if (vInt2 == vInt)
{
hit.node = triangleMeshNode;
return false;
}
vInt2 = (VInt3)triangleMeshNode.ClosestPointOnNode((Vector3)vInt2);
hit.origin = vInt2;
if (!triangleMeshNode.Walkable)
{
hit.point = vInt2;
hit.tangentOrigin = vInt2;
return true;
}
List<VInt3> list = ListPool<VInt3>.Claim();
List<VInt3> list2 = ListPool<VInt3>.Claim();
int num = 0;
while (true)
{
num++;
if (num > 2000)
{
break;
}
TriangleMeshNode triangleMeshNode2 = null;
if (trace != null)
{
trace.Add(triangleMeshNode);
}
if (triangleMeshNode.ContainsPoint(vInt))
{
goto Block_9;
}
for (int i = 0; i < triangleMeshNode.connections.Length; i++)
{
if (triangleMeshNode.connections[i].GraphIndex == triangleMeshNode.GraphIndex)
{
list.Clear();
list2.Clear();
if (triangleMeshNode.GetPortal(triangleMeshNode.connections[i], list, list2, false))
{
VInt3 vInt3 = list.get_Item(0);
VInt3 vInt4 = list2.get_Item(0);
if (Polygon.LeftNotColinear(vInt3, vInt4, hit.origin) || !Polygon.LeftNotColinear(vInt3, vInt4, tmp_end))
{
float num2;
float num3;
if (Polygon.IntersectionFactor(vInt3, vInt4, hit.origin, tmp_end, out num2, out num3))
{
if (num3 >= 0f)
{
if (num2 >= 0f && num2 <= 1f)
{
triangleMeshNode2 = (triangleMeshNode.connections[i] as TriangleMeshNode);
break;
}
}
}
}
}
}
}
if (triangleMeshNode2 == null)
{
goto Block_18;
}
triangleMeshNode = triangleMeshNode2;
}
Debug.LogError("Linecast was stuck in infinite loop. Breaking.");
ListPool<VInt3>.Release(list);
ListPool<VInt3>.Release(list2);
return true;
Block_9:
ListPool<VInt3>.Release(list);
ListPool<VInt3>.Release(list2);
return false;
Block_18:
int vertexCount = triangleMeshNode.GetVertexCount();
for (int j = 0; j < vertexCount; j++)
{
VInt3 vertex = triangleMeshNode.GetVertex(j);
VInt3 vertex2 = triangleMeshNode.GetVertex((j + 1) % vertexCount);
if (Polygon.LeftNotColinear(vertex, vertex2, hit.origin) || !Polygon.LeftNotColinear(vertex, vertex2, tmp_end))
{
VFactor vFactor;
VFactor vFactor2;
if (Polygon.IntersectionFactor(vertex, vertex2, hit.origin, tmp_end, out vFactor, out vFactor2))
{
if (!vFactor2.IsNegative)
{
if (!vFactor.IsNegative && vFactor.nom / vFactor.den <= 1L)
{
VInt3 vInt5 = (vertex2 - vertex) * (float)vFactor.nom;
vInt5 = IntMath.Divide(vInt5, vFactor.den);
vInt5 += vertex;
hit.point = vInt5;
hit.node = triangleMeshNode;
hit.tangent = vertex2 - vertex;
hit.tangentOrigin = vertex;
ListPool<VInt3>.Release(list);
ListPool<VInt3>.Release(list2);
return true;
}
}
}
}
}
Debug.LogWarning("Linecast failing because point not inside node, and line does not hit any edges of it");
ListPool<VInt3>.Release(list);
ListPool<VInt3>.Release(list2);
return false;
}
public new bool Linecast(Vector3 _a, Vector3 _b, GraphNode hint, out GraphHitInfo hit)
{
return(this.SnappedLinecast(_a, _b, hint, out hit));
}
public bool Linecast(VInt3 origin, VInt3 end, GraphNode hint, out GraphHitInfo hit)
{
return NavMeshGraph.Linecast(this, origin, end, hint, out hit, null);
}
public bool Linecast(Vector3 origin, Vector3 end, GraphNode hint, out GraphHitInfo hit)
{
return(NavMeshGraph.Linecast(this, 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 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 static bool Linecast(INavmesh graph, Vector3 tmp_origin, Vector3 tmp_end, GraphNode hint, out GraphHitInfo hit)
{
return(NavMeshGraph.Linecast(graph, tmp_origin, tmp_end, hint, out hit, null));
}
/** Returns if \a _b is visible from \a _a 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
* \param [in] thick An experimental feature can be enabled to use thick linecasts, does not always work as expected
* \param [in] thickness Thickness of the thick linecast
* 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, Node hint, bool thick, float thickness, out GraphHitInfo hit)
{
Int3 end = (Int3)tmp_end;
Int3 origin = (Int3)tmp_origin;
//bool thick = true;
//float thickness = 2F;
if (thickness <= 0) {
thick = false;
}
//System.DateTime startTime = System.DateTime.UtcNow;
hit = new GraphHitInfo ();
MeshNode node = hint as MeshNode;
if (node == null) {
Debug.LogError ("NavMeshGenerator:Linecast: The 'hint' must be a MeshNode");
return true;
}
if (origin == end) {
hit.node = node;
return false;
}
Int3[] vertices = graph.vertices;
origin = (Int3)node.ClosestPoint ((Vector3)origin, vertices);
hit.origin = (Vector3)origin;
Vector3 direction = (Vector3)(end-origin);
Vector3 normalizedDir = direction.normalized;
Int3 normalizedIntDir = (Int3)(normalizedDir*(1000.0f / Int3.Precision));
/*if (thick) {
Vector3 normal = Vector3.Cross (direction,Vector3.up).normalized;
//Debug.DrawLine (tmp_origin+normal*thickness,tmp_end+normal*thickness,Color.black);
//Debug.DrawLine (tmp_origin-normal*thickness,tmp_end-normal*thickness,Color.black);
} else {
//Debug.DrawLine (tmp_origin-Vector3.up,tmp_end-Vector3.up,Color.black);
}*/
#if ASTARDEBUG
Debug.DrawLine ((Vector3)origin,(Vector3)end,Color.black);
#endif
//Current position for tracing
Int3 currentPosition = origin;
//Intersected nodes
int count = 0;
//Temporary variables
int[] vs = new int[3];
//Reason for termination
string reason = "";
MeshNode previousNode = null;
if (!node.walkable) {
reason += " Node is unwalkable";
hit.point = (Vector3)origin;
hit.tangentOrigin = (Vector3)origin;
return true;
}
while (true) {
count++;
//Because of floating point errors, this can actually return true sometimes... I think
if (currentPosition == end) {
reason += " Current == End";
break;
}
if (ContainsPoint (node,(Vector3)end,vertices)) {
reason = "Contains point";
break;
}
if (count > 200) {
Debug.DrawRay ((Vector3)origin,normalizedDir*100,Color.cyan);
Debug.DrawRay ((Vector3)currentPosition,normalizedDir*100,Color.cyan);
Debug.LogError ("Possible infinite loop, intersected > 200 nodes");
Debug.Break ();
return true;
}
//Loop through all vertices
for (int i=0;i<3;i++) {
if (vertices[node[i]] == currentPosition) {
vs[i] = 0;
continue;
}
int tmp = Int3.Dot (normalizedIntDir, (vertices[node[i]]-currentPosition).NormalizeTo (1000));
vs[i] = tmp;
#if ASTARDEBUG
Debug.DrawRay (Vector3.Lerp ((Vector3)vertices[node[i]],(Vector3)node.position,0.5F),Vector3.up*2*tmp*0.001F,new Color (1,0.5F,0));
#endif
}
int max = 0;
if (vs[1] > vs[max]) max = 1;
if (vs[2] > vs[max]) max = 2;
int v1 = node[max];
int v2 = 0;
if (count == 70 || count == 71) {
string s2 = "Count "+count+" "+node.position+"\n";
for (int i=0;i<vs.Length;i++) {
s2 += vs[i].ToString ("0.00")+"\n";
}
Debug.Log (s2);
}
#if ASTARDEBUG
Debug.DrawLine ((Vector3)node.position+Vector3.up*count,(Vector3)vertices[v1]+Vector3.up*count,Color.yellow);
Debug.DrawRay ((Vector3)vertices[v1]+Vector3.up*count,Vector3.up,Color.yellow);
#endif
int preNodeV2 = 0;
long triangleArea = Polygon.TriangleArea2 (currentPosition,currentPosition+normalizedIntDir,vertices[v1]);
if (triangleArea == 0) {
//Polygon.IsColinear (currentPosition,currentPosition+normalizedIntDir,vertices[v1])) {
int max2 = -1;
for (int i=0;i<3;i++) {
if (max != i && (max2 == -1 || vs[i] > vs[max2])) {
max2 = i;
}
}
v2 = node[max2];
} else if (triangleArea < 0) {
//if (Polygon.Left (currentPosition,currentPosition+normalizedIntDir,vertices[v1])) {
preNodeV2 = max - 1 < 0 ? 2 : max-1;
v2 = node[preNodeV2];
} else {
preNodeV2 = max + 1 > 2 ? 0 : max+1;
v2 = node[preNodeV2];
}
Vector3 intersectionPoint;
bool intersectionSuccess = true;
if (thick) {
//Vector3 intersectionPoint = Polygon.IntersectionPoint (currentPosition,end,vertices[v1],vertices[v2]);
float intersectionFactor = Polygon.IntersectionFactor ((Vector3)vertices[v1],(Vector3)vertices[v2],(Vector3)currentPosition,(Vector3)end);
if (intersectionFactor < 0 || intersectionFactor > 1) {
Debug.LogError ("This should not happen");
hit.point = intersectionFactor < 0 ? (Vector3)vertices[v1] : (Vector3)vertices[v2];
return true;
}
Vector3 dir2 = (Vector3)(vertices[v2]-vertices[v1]);
intersectionPoint = (Vector3)vertices[v1]+dir2*intersectionFactor;
float magn = dir2.magnitude;
intersectionFactor *= magn;
if (intersectionFactor-thickness < 0) {
hit.point = (Vector3)vertices[v1];
return true;
} else if (intersectionFactor+thickness > magn) {
hit.point = (Vector3)vertices[v2];
return true;
}
} else {
float intersectionFactor = Polygon.IntersectionFactor ((Vector3)vertices[v1],(Vector3)vertices[v2],(Vector3)currentPosition,(Vector3)end);
//Lines were colinear
if (intersectionFactor == -1) {
intersectionSuccess = false;
}
intersectionFactor = Mathf.Clamp01 (intersectionFactor);
intersectionPoint = (Vector3)vertices[v1] + (Vector3)(vertices[v2]-vertices[v1])*intersectionFactor;
if (!intersectionSuccess) {
if ((vertices[v1]-currentPosition).sqrMagnitude >= (end-currentPosition).sqrMagnitude) {
intersectionPoint = (Vector3)end;
reason = "Colinear - Aborting";
break;
} else {
preNodeV2 = max != 0 && preNodeV2 != 0 ? 0 : (max != 1 && preNodeV2 != 1 ? 1 : 2);
v2 = node[preNodeV2];
intersectionPoint = (Vector3)vertices[v1];
intersectionSuccess = true;
reason = "Colinear - Continuing";
}
}
float distanceFactor = Mathfx.NearestPointFactor ((Vector3)origin,(Vector3)end,intersectionPoint);
if (distanceFactor > 1F) {
intersectionSuccess = false;
}
}
#if ASTARDEBUG
Debug.DrawLine ((Vector3)vertices[v1]+Vector3.up*count,(Vector3)vertices[v2]+Vector3.up*count,Color.magenta);
#endif
MeshNode nextNode = null;
bool breakOutFromLoop = false;
for (int i=0;i<node.connections.Length;i++) {
MeshNode other = node.connections[i] as MeshNode;
//Make sure the node is a MeshNode and that it doesn't of some reason link to itself.
if (other == null || other == node) {
continue;
}
int matches = 0;
for (int v=0;v<3;v++) {
if (other[v] == v1 || other[v] == v2) {
matches++;
}
}
if (matches == 2) {
//The node is the previous node, the endpoint must be in between the nodes (floating point error), abort
if (other == previousNode) {
reason += "Other == previous node\n";
breakOutFromLoop = true;
break;
} else {
nextNode = other;
}
break;
}
}
if (breakOutFromLoop) {
break;
}
if (nextNode == null || !intersectionSuccess || !nextNode.walkable) {
if (nextNode == null) {
reason += "No next node (wall)";
}
#if ASTARDEBUG
Debug.DrawLine ((Vector3)origin,(Vector3)intersectionPoint,Color.red);
#endif
hit.tangentOrigin = (Vector3)vertices[v1];
hit.tangent = (Vector3)(vertices[v2]-vertices[v1]);
hit.point = intersectionPoint;
hit.node = node;
return true;
} else {
#if ASTARDEBUG
Debug.DrawLine ((Vector3)node.position+Vector3.up*count,(Vector3)nextNode.position+Vector3.up*(count+1),Color.green);
#endif
previousNode = node;
node = nextNode;
currentPosition = (Int3)intersectionPoint;
}
}
#if ASTARDEBUG
Debug.DrawLine ((Vector3)origin,(Vector3)end,Color.green);
#endif
hit.node = node;
return false;
}
/** Returns if \a _b is visible from \a _a 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
* and it uses Bresenham's line drawing algorithm as opposed to the others which use sampling at fixed intervals.
* If you only care about if one \b node can see another \b node, then this function is great, but if you need more precision than one node,
* use the normal linecast functions
* \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) 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 new bool SnappedLinecast(Vector3 _a, Vector3 _b, Node hint, out GraphHitInfo hit)
{
hit = new GraphHitInfo ();
//System.DateTime startTime = System.DateTime.UtcNow;
LevelGridNode n1 = GetNearest (_a,NNConstraint.None).node as LevelGridNode;
LevelGridNode n2 = GetNearest (_b,NNConstraint.None).node as LevelGridNode;
if (n1 == null || n2 == null) {
hit.node = null;
hit.point = _a;
return true;
}
_a = inverseMatrix.MultiplyPoint3x4 ((Vector3)n1.position);
_a.x -= 0.5F;
_a.z -= 0.5F;
_b = inverseMatrix.MultiplyPoint3x4 ((Vector3)n2.position);
_b.x -= 0.5F;
_b.z -= 0.5F;
Int3 a = new Int3 (Mathf.RoundToInt (_a.x),Mathf.RoundToInt (_a.y),Mathf.RoundToInt (_a.z));
Int3 b = new Int3 (Mathf.RoundToInt (_b.x),Mathf.RoundToInt (_b.y),Mathf.RoundToInt (_b.z));
hit.origin = (Vector3)a;
//Debug.DrawLine (matrix.MultiplyPoint3x4 (a*100),matrix.MultiplyPoint3x4 (b*100),Color.yellow);
if (!n1.walkable) {//nodes[a.z*width+a.x].walkable) {
hit.node = n1;//nodes[a.z*width+a.x];
hit.point = matrix.MultiplyPoint3x4 (new Vector3 (a.x+0.5F,0,a.z+0.5F));
hit.point.y = ((Vector3)hit.node.position).y;
return true;
}
int dx = Mathf.Abs (a.x-b.x);
int dz = Mathf.Abs (a.z-b.z);
LevelGridNode currentNode = n1;
while (true) {
if (currentNode == n2) { //a.x == b.x && a.z == b.z) {
//System.DateTime endTime2 = System.DateTime.UtcNow;
//float theTime2 = (endTime2-startTime).Ticks*0.0001F;
//Debug.Log ("Grid Linecast : Time "+theTime2.ToString ("0.00"));
return false;
}
//The nodes are at the same position in the graph when seen from above
if (currentNode.GetIndex() == n2.GetIndex()) {
hit.node = currentNode;
hit.point = (Vector3)currentNode.position;
return true;
}
dx = System.Math.Abs(a.x-b.x);
dz = System.Math.Abs(a.z-b.z);
int dir = 0;
if (dx >= dz) {
dir = b.x>a.x ? 1 : 3;
} else if (dz > dx) {
dir = b.z>a.z ? 2 : 0;
}
if (CheckConnection (currentNode,dir)) {
LevelGridNode other = nodes[currentNode.GetIndex()+neighbourOffsets[dir] + width*depth*currentNode.GetConnectionValue(dir)] as LevelGridNode;
if (!other.walkable) {
hit.node = other;
hit.point = (Vector3)other.position;
return true;
}
//Debug.DrawLine (matrix.MultiplyPoint3x4 (a*100),matrix.MultiplyPoint3x4 (newPos*100));
a = (Int3)inverseMatrix.MultiplyPoint3x4 ((Vector3)other.position);
currentNode = other;
} else {
hit.node = currentNode;
hit.point = (Vector3)currentNode.position;//matrix.MultiplyPoint3x4 (new Vector3 (a.x+0.5F,0,a.z+0.5F));
return true;
}
/*int e2 = err*2;
Int3 newPos = a;
if (e2 > -dz) {
err = err-dz;
dir = sx;
newPos.x += sx;
}
if (e2 < dx) {
err = err+dx;
dir += width*sz;
newPos.z += sz;
}
if (dir == 0) {
Debug.LogError ("Offset is zero, this should not happen");
return false;
}
for (int i=0;i<neighbourOffsets.Length;i++) {
if (neighbourOffsets[i] == dir) {
if (CheckConnection (nodes[a.z*width+a.x] as LevelGridNode,i)) {
if (!nodes[newPos.z*width+newPos.x].walkable) {
hit.node = nodes[a.z*width+a.x];
hit.point = matrix.MultiplyPoint3x4 (new Vector3 (a.x+0.5F,0,a.z+0.5F));
hit.point.y = ((Vector3)hit.node.position).y;
return true;
}
//Debug.DrawLine (matrix.MultiplyPoint3x4 (a*100),matrix.MultiplyPoint3x4 (newPos*100));
a = newPos;
break;
} else {
hit.node = nodes[a.z*width+a.x];
hit.point = matrix.MultiplyPoint3x4 (new Vector3 (a.x+0.5F,0,a.z+0.5F));
hit.point.y = ((Vector3)hit.node.position).y;
return true;
}
}
}*/
}
//Debug.DrawLine (_a,_b,Color.green);
//hit.success = true;
}
/** 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 = Pathfinding.Util.ListPool <Vector3> .Claim(); //new List<Vector3>(1);
List <Vector3> right = Pathfinding.Util.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.");
Pathfinding.Util.ListPool <Vector3> .Release(left);
Pathfinding.Util.ListPool <Vector3> .Release(right);
return(true);
}
TriangleMeshNode newNode = null;
if (trace != null)
{
trace.Add(node);
}
if (node.ContainsPoint(end))
{
Pathfinding.Util.ListPool <Vector3> .Release(left);
Pathfinding.Util.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;
Pathfinding.Util.ListPool <Vector3> .Release(left);
Pathfinding.Util.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");
Pathfinding.Util.ListPool <Vector3> .Release(left);
Pathfinding.Util.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] _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 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
* 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) {
return Linecast (this as INavmesh, origin,end,hint, out hit, null);
}
/** Returns if \a _b is visible from \a _a 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
* and it uses Bresenham's line drawing algorithm as opposed to the others which use sampling at fixed intervals.
* If you only care about if one \b node can see another \b node, then this function is great, but if you need more precision than one node,
* use the normal linecast functions
*
* \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 SnappedLinecast (Vector3 _a, Vector3 _b, GraphNode hint, out GraphHitInfo hit) {
hit = new GraphHitInfo ();
//System.DateTime startTime = System.DateTime.UtcNow;
GraphNode n1 = GetNearest (_a,NNConstraint.None).node;
GraphNode n2 = GetNearest (_b,NNConstraint.None).node;
_a = inverseMatrix.MultiplyPoint3x4 ((Vector3)n1.position);
_a.x -= 0.5F;
_a.z -= 0.5F;
_b = inverseMatrix.MultiplyPoint3x4 ((Vector3)n2.position);
_b.x -= 0.5F;
_b.z -= 0.5F;
Int3 a = new Int3 (Mathf.RoundToInt (_a.x),Mathf.RoundToInt (_a.y),Mathf.RoundToInt (_a.z));
Int3 b = new Int3 (Mathf.RoundToInt (_b.x),Mathf.RoundToInt (_b.y),Mathf.RoundToInt (_b.z));
hit.origin = (Vector3)a;
//Debug.DrawLine (matrix.MultiplyPoint3x4 (a*100),matrix.MultiplyPoint3x4 (b*100),Color.yellow);
if (!nodes[a.z*width+a.x].Walkable) {
hit.node = nodes[a.z*width+a.x];
hit.point = matrix.MultiplyPoint3x4 (new Vector3 (a.x+0.5F,0,a.z+0.5F));
hit.point.y = ((Vector3)hit.node.position).y;
return true;
}
int dx = Mathf.Abs (a.x-b.x);
int dz = Mathf.Abs (a.z-b.z);
int sx = 0;
int sz = 0;
if (a.x < b.x) {
sx = 1;
} else {
sx = -1;
}
if (a.z < b.z) {
sz = 1;
} else {
sz = -1;
}
int err = dx-dz;
while (true) {
if (a.x == b.x && a.z == b.z) {
//System.DateTime endTime2 = System.DateTime.UtcNow;
//float theTime2 = (endTime2-startTime).Ticks*0.0001F;
//Debug.Log ("Grid Linecast : Time "+theTime2.ToString ("0.00"));
return false;
}
int e2 = err*2;
int dir = 0;
Int3 newPos = a;
if (e2 > -dz) {
err = err-dz;
dir = sx;
newPos.x += sx;
}
if (e2 < dx) {
err = err+dx;
dir += width*sz;
newPos.z += sz;
}
if (dir == 0) {
Debug.LogError ("Offset is zero, this should not happen");
return false;
}
for (int i=0;i<neighbourOffsets.Length;i++) {
if (neighbourOffsets[i] == dir) {
if (CheckConnection (nodes[a.z*width+a.x] as GridNode,i)) {
if (!nodes[newPos.z*width+newPos.x].Walkable) {
hit.node = nodes[a.z*width+a.x];
hit.point = matrix.MultiplyPoint3x4 (new Vector3 (a.x+0.5F,0,a.z+0.5F));
hit.point.y = ((Vector3)hit.node.position).y;
return true;
}
//Debug.DrawLine (matrix.MultiplyPoint3x4 (a*100),matrix.MultiplyPoint3x4 (newPos*100));
a = newPos;
break;
} else {
hit.node = nodes[a.z*width+a.x];
hit.point = matrix.MultiplyPoint3x4 (new Vector3 (a.x+0.5F,0,a.z+0.5F));
hit.point.y = ((Vector3)hit.node.position).y;
return true;
}
}
}
}
//Debug.DrawLine (_a,_b,Color.green);
//hit.success = true;
//return false;
}
public bool Linecast(Vector3 origin, Vector3 end, GraphNode hint, out GraphHitInfo hit)
{
return NavMeshGraph.Linecast(this, origin, end, hint, out hit, null);
}
