public static bool Left (Int2 a, Int2 b, Int2 p) { return VectorMath.RightOrColinear(a, b, p); }
/** Returns if the line segment \a a2 - \a b2 intersects the line segment \a a - \a b. * If only the endpoints coincide, the result is undefined (may be true or false). */ public static bool Intersects (Int2 a, Int2 b, Int2 a2, Int2 b2) { return Left (a,b,a2) != Left (a,b,b2) && Left (a2,b2,a) != Left (a2,b2,b); }
/** Returns if the triangle \a ABC contains the point \a p. * The triangle vertices are assumed to be laid out in clockwise order. */ public static bool ContainsPoint (Int2 a, Int2 b, Int2 c, Int2 p) { return VectorMath.IsClockwiseOrColinear(a, b, p) && VectorMath.IsClockwiseOrColinear(b, c, p) && VectorMath.IsClockwiseOrColinear(c, a, p); }
/** Dot product of the two coordinates */ public static long DotLong (Int2 a, Int2 b) { return (long)a.x*(long)b.x + (long)a.y*(long)b.y; }
/** Factor of the nearest point on the segment. * Returned value is in the range [0,1] if the point lies on the segment otherwise it just lies on the line. * The closest point can be got by (end-start)*factor + start; */ public static float NearestPointFactor (Int2 lineStart, Int2 lineEnd, Int2 point) { var lineDirection = lineEnd-lineStart; double magn = lineDirection.sqrMagnitudeLong; double closestPoint = Int2.DotLong(point-lineStart,lineDirection); //Vector3.Dot(lineDirection,lineDirection); if (magn != 0) closestPoint /= magn; return (float)closestPoint; //return closestPoint / magn; }
public static float NearestPointFactor (Int2 lineStart, Int2 lineEnd, Int2 point) { return VectorMath.ClosestPointOnLineFactor(lineStart, lineEnd, point); }
private static IntRect NodeBounds(MeshNode[] nodes, int from, int to) { if (to - from <= 0) { throw new ArgumentException(); } Int3 vertex = nodes[from].GetVertex(0); Int2 @int = new Int2(vertex.x, vertex.z); Int2 int2 = @int; for (int i = from; i < to; i++) { MeshNode meshNode = nodes[i]; for (int j = 1; j < meshNode.GetVertexCount(); j++) { Int3 vertex2 = meshNode.GetVertex(j); @int.x = Math.Min(@int.x, vertex2.x); @int.y = Math.Min(@int.y, vertex2.z); int2.x = Math.Max(int2.x, vertex2.x); int2.y = Math.Max(int2.y, vertex2.z); } } return new IntRect(@int.x, @int.y, int2.x, int2.y); }
public static bool IsClockwiseMargin(Int2 a, Int2 b, Int2 c) { return((long)(b.x - a.x) * (long)(c.y - a.y) - (long)(c.x - a.x) * (long)(b.y - a.y) <= 0L); }
public static Int2 Rotate(Int2 v, int r) { r = r % 4; return(new Int2(v.x * Rotations[r * 4 + 0] + v.y * Rotations[r * 4 + 1], v.x * Rotations[r * 4 + 2] + v.y * Rotations[r * 4 + 3])); }
public static Int2 Min (Int2 a, Int2 b) { return new Int2 (Math.Min (a.x,b.x), Math.Min (a.y,b.y)); }
public static float NearestPointFactor(Int2 lineStart, Int2 lineEnd, Int2 point) { return(VectorMath.ClosestPointOnLineFactor(lineStart, lineEnd, point)); }
public static bool Intersects(Int2 a, Int2 b, Int2 a2, Int2 b2) { return Polygon.Left(a, b, a2) != Polygon.Left(a, b, b2) && Polygon.Left(a2, b2, a) != Polygon.Left(a2, b2, b); }
public static int Dot(Int2 a, Int2 b) { return(a.x * b.x + a.y * b.y); }
public IntRect Offset(Int2 offset) { return(new IntRect(this.xmin + offset.x, this.ymin + offset.y, this.xmax + offset.x, this.ymax + offset.y)); }
public static bool Intersects (Int2 start1, Int2 end1, Int2 start2, Int2 end2) { return VectorMath.SegmentsIntersect(start1, end1, start2, end2); }
public static Int2 Min(Int2 a, Int2 b) { return(new Int2(System.Math.Min(a.x, b.x), System.Math.Min(a.y, b.y))); }
/** Returns true if the points a in a clockwise order or if they are colinear */ public static bool IsClockwiseOrColinear (Int2 a, Int2 b, Int2 c) { return RightOrColinear(a, b, c); }
public static bool Left(Int2 a, Int2 b, Int2 p) { return(VectorMath.RightOrColinear(a, b, p)); }
/** Returns true if the points a in a clockwise order or if they are colinear */ public static bool IsClockwiseMargin (Int2 a, Int2 b, Int2 c) { return Left(a, b, c); }
public static bool Intersects(Int2 start1, Int2 end1, Int2 start2, Int2 end2) { return(VectorMath.SegmentsIntersect(start1, end1, start2, end2)); }
/** Calculates the bounding box in XZ space of all nodes between \a from (inclusive) and \a to (exclusive) */ static IntRect NodeBounds (MeshNode[] nodes, int from, int to) { if (to - from <= 0) throw new ArgumentException(); var first = nodes[from].GetVertex(0); var min = new Int2(first.x,first.z); Int2 max = min; for (int j = from; j < to; j++) { var node = nodes[j]; var nverts = node.GetVertexCount(); for (int i = 0; i < nverts; i++) { var p = node.GetVertex(i); min.x = Math.Min (min.x, p.x); min.y = Math.Min (min.y, p.z); max.x = Math.Max (max.x, p.x); max.y = Math.Max (max.y, p.z); } } return new IntRect (min.x, min.y, max.x, max.y); }
public static bool ContainsPoint(Int2 a, Int2 b, Int2 c, Int2 p) { return(VectorMath.IsClockwiseOrColinear(a, b, p) && VectorMath.IsClockwiseOrColinear(b, c, p) && VectorMath.IsClockwiseOrColinear(c, a, p)); }
public static Int2 Max (Int2 a, Int2 b) { return new Int2(System.Math.Max(a.x, b.x), System.Math.Max(a.y, b.y)); }
public static long DotLong(Int2 a, Int2 b) { return((long)a.x * (long)b.x + (long)a.y * (long)b.y); }
/** Returns if \a p lies on the left side of the line \a a - \a b. Also returns true if the points are colinear */ public static bool Left (Int2 a, Int2 b, Int2 c) { return (long)(b.x - a.x) * (long)(c.y - a.y) - (long)(c.x - a.x) * (long)(b.y - a.y) <= 0; }
public static Int2 Rotate(Int2 v, int r) { r %= 4; return(new Int2(v.x * Int2.Rotations[r * 4] + v.y * Int2.Rotations[r * 4 + 1], v.x * Int2.Rotations[r * 4 + 2] + v.y * Int2.Rotations[r * 4 + 3])); }
public static Int2 Rotate(Int2 v, int r) { r = r % 4; return(new Int2((v.x * Rotations[r * 4]) + (v.y * Rotations[(r * 4) + 1]), (v.x * Rotations[(r * 4) + 2]) + (v.y * Rotations[(r * 4) + 3]))); }
public static Int2 Max(Int2 a, Int2 b) { return(new Int2(Math.Max(a.x, b.x), Math.Max(a.y, b.y))); }
/** Factor of the nearest point on the segment. * Returned value is in the range [0,1] if the point lies on the segment otherwise it just lies on the line. * The closest point can be calculated using (end-start)*factor + start; */ public static float ClosestPointOnLineFactor (Int2 lineStart, Int2 lineEnd, Int2 point) { var lineDirection = lineEnd - lineStart; double magn = lineDirection.sqrMagnitudeLong; double closestPoint = Int2.DotLong(point - lineStart, lineDirection); if (magn != 0) closestPoint /= magn; return (float)closestPoint; }
public static Int3 ToInt3XZ(Int2 o) { return(new Int3(o.x, 0, o.y)); }
public static bool IsClockwiseMargin (Int2 a, Int2 b, Int2 c) { return VectorMath.IsClockwiseOrColinear(a, b, c); }
/** Async method for moving the graph */ IEnumerator UpdateGraphCoroutine() { // Find the direction that we want to move the graph in. // Calcuculate this in graph space (where a distance of one is the size of one node) Vector3 dir = PointToGraphSpace(target.position) - PointToGraphSpace(graph.center); // Snap to a whole number of nodes dir.x = Mathf.Round(dir.x); dir.z = Mathf.Round(dir.z); dir.y = 0; // Nothing do to if (dir == Vector3.zero) { yield break; } // Number of nodes to offset in each direction Int2 offset = new Int2(-Mathf.RoundToInt(dir.x), -Mathf.RoundToInt(dir.z)); // Move the center (this is in world units, so we need to convert it back from graph space) graph.center += graph.transform.TransformVector(dir); graph.UpdateTransform(); // Cache some variables for easier access int width = graph.width; int depth = graph.depth; GridNodeBase[] nodes; // Layers are required when handling LayeredGridGraphs int layers = graph.LayerCount; var layeredGraph = graph as LayerGridGraph; if (layeredGraph != null) { nodes = layeredGraph.nodes; } else { nodes = graph.nodes; } // Create a temporary buffer required for the calculations if (buffer == null || buffer.Length != width * depth) { buffer = new GridNodeBase[width * depth]; } // Check if we have moved less than a whole graph width all directions // If we have moved more than this we can just as well recalculate the whole graph if (Mathf.Abs(offset.x) <= width && Mathf.Abs(offset.y) <= depth) { IntRect recalculateRect = new IntRect(0, 0, offset.x, offset.y); // If offset.x < 0, adjust the rect if (recalculateRect.xmin > recalculateRect.xmax) { int tmp2 = recalculateRect.xmax; recalculateRect.xmax = width + recalculateRect.xmin; recalculateRect.xmin = width + tmp2; } // If offset.y < 0, adjust the rect if (recalculateRect.ymin > recalculateRect.ymax) { int tmp2 = recalculateRect.ymax; recalculateRect.ymax = depth + recalculateRect.ymin; recalculateRect.ymin = depth + tmp2; } // Connections need to be recalculated for the neighbours as well, so we need to expand the rect by 1 var connectionRect = recalculateRect.Expand(1); // Makes sure the rect stays inside the grid connectionRect = IntRect.Intersection(connectionRect, new IntRect(0, 0, width, depth)); // Offset each node by the #offset variable // nodes which would end up outside the graph // will wrap around to the other side of it for (int l = 0; l < layers; l++) { int layerOffset = l * width * depth; for (int z = 0; z < depth; z++) { int pz = z * width; int tz = ((z + offset.y + depth) % depth) * width; for (int x = 0; x < width; x++) { buffer[tz + ((x + offset.x + width) % width)] = nodes[layerOffset + pz + x]; } } yield return(null); // Copy the nodes back to the graph // and set the correct indices for (int z = 0; z < depth; z++) { int pz = z * width; for (int x = 0; x < width; x++) { int newIndex = pz + x; var node = buffer[newIndex]; if (node != null) { node.NodeInGridIndex = newIndex; } nodes[layerOffset + newIndex] = node; } // Calculate the limits for the region that has been wrapped // to the other side of the graph int xmin, xmax; if (z >= recalculateRect.ymin && z < recalculateRect.ymax) { xmin = 0; xmax = depth; } else { xmin = recalculateRect.xmin; xmax = recalculateRect.xmax; } for (int x = xmin; x < xmax; x++) { var node = buffer[pz + x]; if (node != null) { // Clear connections on all nodes that are wrapped and placed on the other side of the graph. // This is both to clear any custom connections (which do not really make sense after moving the node) // and to prevent possible exceptions when the node will later (possibly) be destroyed because it was // not needed anymore (only for layered grid graphs). node.ClearConnections(false); } } } yield return(null); } // The calculation will only update approximately this number of // nodes per frame. This is used to keep CPU load per frame low int yieldEvery = 1000; // To avoid the update taking too long, make yieldEvery somewhat proportional to the number of nodes that we are going to update int approxNumNodesToUpdate = Mathf.Max(Mathf.Abs(offset.x), Mathf.Abs(offset.y)) * Mathf.Max(width, depth); yieldEvery = Mathf.Max(yieldEvery, approxNumNodesToUpdate / 10); int counter = 0; // Recalculate the nodes // Take a look at the image in the docs for the UpdateGraph method // to see which nodes are being recalculated. for (int z = 0; z < depth; z++) { int xmin, xmax; if (z >= recalculateRect.ymin && z < recalculateRect.ymax) { xmin = 0; xmax = width; } else { xmin = recalculateRect.xmin; xmax = recalculateRect.xmax; } for (int x = xmin; x < xmax; x++) { graph.RecalculateCell(x, z, false, false); } counter += (xmax - xmin); if (counter > yieldEvery) { counter = 0; yield return(null); } } for (int z = 0; z < depth; z++) { int xmin, xmax; if (z >= connectionRect.ymin && z < connectionRect.ymax) { xmin = 0; xmax = width; } else { xmin = connectionRect.xmin; xmax = connectionRect.xmax; } for (int x = xmin; x < xmax; x++) { graph.CalculateConnections(x, z); } counter += (xmax - xmin); if (counter > yieldEvery) { counter = 0; yield return(null); } } yield return(null); // Calculate all connections for the nodes along the boundary // of the graph, these always need to be updated /** \todo Optimize to not traverse all nodes in the graph, only those at the edges */ for (int z = 0; z < depth; z++) { for (int x = 0; x < width; x++) { if (x == 0 || z == 0 || x == width - 1 || z == depth - 1) { graph.CalculateConnections(x, z); } } } // We need to clear the Area if we are not using flood filling. // This will make pathfinding always work, but it may be slow // to figure out that no path exists if none does. // (of course, if there are regions between which no valid // paths exist, then the #floodFill field should not // be set to false anyway). if (!floodFill) { graph.GetNodes(node => node.Area = 1); } } else { // The calculation will only update approximately this number of // nodes per frame. This is used to keep CPU load per frame low int yieldEvery = Mathf.Max(depth * width / 20, 1000); int counter = 0; // Just update all nodes for (int z = 0; z < depth; z++) { for (int x = 0; x < width; x++) { graph.RecalculateCell(x, z); } counter += width; if (counter > yieldEvery) { counter = 0; yield return(null); } } // Recalculate the connections of all nodes for (int z = 0; z < depth; z++) { for (int x = 0; x < width; x++) { graph.CalculateConnections(x, z); } counter += width; if (counter > yieldEvery) { counter = 0; yield return(null); } } } }
/** Returns if \a p lies on the right side of the line \a a - \a b. * Also returns true if the points are colinear. */ public static bool RightOrColinear (Int2 a, Int2 b, Int2 p) { return (long)(b.x - a.x) * (long)(p.y - a.y) - (long)(p.x - a.x) * (long)(b.y - a.y) <= 0; }
public static bool Left(Int2 a, Int2 b, Int2 c) { return((long)(b.x - a.x) * (long)(c.y - a.y) - (long)(c.x - a.x) * (long)(b.y - a.y) <= 0L); }
/** Returns if the line segment \a start2 - \a end2 intersects the line segment \a start1 - \a end1. * If only the endpoints coincide, the result is undefined (may be true or false). */ public static bool SegmentsIntersect (Int2 start1, Int2 end1, Int2 start2, Int2 end2) { return RightOrColinear(start1, end1, start2) != RightOrColinear(start1, end1, end2) && RightOrColinear(start2, end2, start1) != RightOrColinear(start2, end2, end1); }
public static bool IsClockwiseMargin(Int2 a, Int2 b, Int2 c) { return(Polygon.Left(a, b, c)); }
/** Create connections between all nodes. * \warning This implementation is not thread safe. It uses cached variables to improve performance */ void CreateNodeConnections (TriangleMeshNode[] nodes) { List<MeshNode> connections = Pathfinding.Util.ListPool<MeshNode>.Claim (); //new List<MeshNode>(); List<uint> connectionCosts = Pathfinding.Util.ListPool<uint>.Claim (); //new List<uint>(); Dictionary<Int2,int> nodeRefs = cachedInt2_int_dict; nodeRefs.Clear(); // Build node neighbours for (int i=0;i<nodes.Length;i++) { TriangleMeshNode node = nodes[i]; int av = node.GetVertexCount (); for (int a=0;a<av;a++) { // Recast can in some very special cases generate degenerate triangles which are simply lines // In that case, duplicate keys might be added and thus an exception will be thrown // It is safe to ignore the second edge though... I think (only found one case where this happens) var key = new Int2 (node.GetVertexIndex(a), node.GetVertexIndex ((a+1) % av)); if (!nodeRefs.ContainsKey(key)) { nodeRefs.Add (key, i); } } } for (int i=0;i<nodes.Length;i++) { TriangleMeshNode node = nodes[i]; connections.Clear (); connectionCosts.Clear (); int av = node.GetVertexCount (); for (int a=0;a<av;a++) { int first = node.GetVertexIndex(a); int second = node.GetVertexIndex((a+1) % av); int connNode; if (nodeRefs.TryGetValue (new Int2 (second, first), out connNode)) { TriangleMeshNode other = nodes[connNode]; int bv = other.GetVertexCount (); for (int b=0;b<bv;b++) { /** \todo This will fail on edges which are only partially shared */ if (other.GetVertexIndex (b) == second && other.GetVertexIndex ((b+1) % bv) == first) { uint cost = (uint)(node.position - other.position).costMagnitude; connections.Add (other); connectionCosts.Add (cost); break; } } } } node.connections = connections.ToArray (); node.connectionCosts = connectionCosts.ToArray (); } Pathfinding.Util.ListPool<MeshNode>.Release (connections); Pathfinding.Util.ListPool<uint>.Release (connectionCosts); }
public static bool Intersects(Int2 a, Int2 b, Int2 a2, Int2 b2) { return(Polygon.Left(a, b, a2) != Polygon.Left(a, b, b2) && Polygon.Left(a2, b2, a) != Polygon.Left(a2, b2, b)); }
public static int Dot (Int2 a, Int2 b) { return a.x*b.x + a.y*b.y; }
public static bool ContainsPoint(Int2 a, Int2 b, Int2 c, Int2 p) { return(Polygon.IsClockwiseMargin(a, b, p) && Polygon.IsClockwiseMargin(b, c, p) && Polygon.IsClockwiseMargin(c, a, p)); }
public BBTreeBox(MeshNode node) { this.node = node; Int3 vertex = node.GetVertex(0); Int2 @int = new Int2(vertex.x, vertex.z); Int2 int2 = @int; for (int i = 1; i < node.GetVertexCount(); i++) { Int3 vertex2 = node.GetVertex(i); @int.x = Math.Min(@int.x, vertex2.x); @int.y = Math.Min(@int.y, vertex2.z); int2.x = Math.Max(int2.x, vertex2.x); int2.y = Math.Max(int2.y, vertex2.z); } this.rect = new IntRect(@int.x, @int.y, int2.x, int2.y); this.left = (this.right = -1); }
public static bool RightOrColinear(Int2 a, Int2 b, Int2 p) { return((long)(b.x - a.x) * (long)(p.y - a.y) - (long)(p.x - a.x) * (long)(b.y - a.y) <= 0L); }
public BBTreeBox (MeshNode node) { this.node = node; var first = node.GetVertex(0); var min = new Int2(first.x,first.z); Int2 max = min; for (int i=1;i<node.GetVertexCount();i++) { var p = node.GetVertex(i); min.x = Math.Min (min.x,p.x); min.y = Math.Min (min.y,p.z); max.x = Math.Max (max.x,p.x); max.y = Math.Max (max.y,p.z); } rect = new IntRect (min.x,min.y,max.x,max.y); left = right = -1; }
public static bool IsClockwiseOrColinear(Int2 a, Int2 b, Int2 c) { return(VectorMath.RightOrColinear(a, b, c)); }
public static Int2 Rotate (Int2 v, int r) { r = r % 4; return new Int2(v.x*Rotations[r*4+0] + v.y*Rotations[r*4+1], v.x*Rotations[r*4+2] + v.y*Rotations[r*4+3]); }
public static bool SegmentsIntersect(Int2 start1, Int2 end1, Int2 start2, Int2 end2) { return(VectorMath.RightOrColinear(start1, end1, start2) != VectorMath.RightOrColinear(start1, end1, end2) && VectorMath.RightOrColinear(start2, end2, start1) != VectorMath.RightOrColinear(start2, end2, end1)); }
public static Int3 ToInt3XZ (Int2 o) { return new Int3(o.x, 0, o.y); }
public static float NearestPointFactor(Int2 lineStart, Int2 lineEnd, Int2 point) { Int2 b = lineEnd - lineStart; double num = (double)b.sqrMagnitudeLong; double num2 = (double)Int2.DotLong(point - lineStart, b); if (num != 0.0) { num2 /= num; } return (float)num2; }
/** Returns if the triangle \a ABC contains the point \a p */ public static bool ContainsPoint (Int2 a, Int2 b, Int2 c, Int2 p) { return IsClockwiseMargin (a,b, p) && IsClockwiseMargin (b,c, p) && IsClockwiseMargin (c,a, p); }
/** Returns a new rect which is offset by the specified amount. */ public IntRect Offset(Int2 offset) { return(new IntRect(xmin + offset.x, ymin + offset.y, xmax + offset.x, ymax + offset.y)); }
/** Returns true if the points a in a clockwise order or if they are colinear */ public static bool IsClockwiseMargin (Int2 a, Int2 b, Int2 c) { return (long)(b.x - a.x) * (long)(c.y - a.y) - (long)(c.x - a.x) * (long)(b.y - a.y) <= 0; }
/** 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; }
/** Returns a new rect which is offset by the specified amount. */ public IntRect Offset ( Int2 offset ) { return new IntRect ( xmin+offset.x, ymin + offset.y, xmax + offset.x, ymax + offset.y ); }
public static long DotLong(Int2 a, Int2 b) { return((a.x * b.x) + (a.y * b.y)); }