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));
}