public static Int2 FromInt3XZ (Int3 o) { return new Int2 (o.x,o.z); }
/** Reset all values to their default values. * * \note All inheriting path types (e.g ConstantPath, RandomPath, etc.) which declare their own variables need to * override this function, resetting ALL their variables to enable recycling of paths. * If this is not done, trying to use that path type for pooling might result in weird behaviour. * The best way is to reset to default values the variables declared in the extended path type and then * call this base function in inheriting types with base.Reset (). * * \warning This function should not be called manually. */ public virtual void Reset () { #if ASTAR_POOL_DEBUG pathTraceInfo = "This path was got from the pool or created from here (stacktrace):\n"; pathTraceInfo += System.Environment.StackTrace; #endif if (System.Object.ReferenceEquals (AstarPath.active, null)) throw new System.NullReferenceException ("No AstarPath object found in the scene. " + "Make sure there is one or do not create paths in Awake"); hasBeenReset = true; state = (int)PathState.Created; releasedNotSilent = false; pathHandler = null; callback = null; _errorLog = ""; pathCompleteState = PathCompleteState.NotCalculated; path = ListPool<GraphNode>.Claim(); vectorPath = ListPool<Vector3>.Claim(); currentR = null; duration = 0; searchIterations = 0; searchedNodes = 0; //calltime nnConstraint = PathNNConstraint.Default; next = null; heuristic = AstarPath.active.heuristic; heuristicScale = AstarPath.active.heuristicScale; enabledTags = -1; tagPenalties = null; callTime = System.DateTime.UtcNow; pathID = AstarPath.active.GetNextPathID (); hTarget = Int3.zero; hTargetNode = null; }
public override bool ContainsPoint (Int3 p) { // Get the object holding the vertex data for this node // This is usually a graph or a recast graph tile INavmeshHolder navmeshHolder = GetNavmeshHolder(GraphIndex); // Get all 3 vertices for this node Int3 a = navmeshHolder.GetVertex(v0); Int3 b = navmeshHolder.GetVertex(v1); Int3 c = navmeshHolder.GetVertex(v2); if ((long)(b.x - a.x) * (long)(p.z - a.z) - (long)(p.x - a.x) * (long)(b.z - a.z) > 0) return false; if ((long)(c.x - b.x) * (long)(p.z - b.z) - (long)(p.x - b.x) * (long)(c.z - b.z) > 0) return false; if ((long)(a.x - c.x) * (long)(p.z - c.z) - (long)(p.x - c.x) * (long)(a.z - c.z) > 0) return false; return true; // Equivalent code, but the above code is faster //return Polygon.IsClockwiseMargin (a,b, p) && Polygon.IsClockwiseMargin (b,c, p) && Polygon.IsClockwiseMargin (c,a, p); //return Polygon.ContainsPoint(g.GetVertex(v0),g.GetVertex(v1),g.GetVertex(v2),p); }
/** Returns the intersection factors for line 1 and line 2. The intersection factors is a distance along the line \a start - \a end where the other line intersects it.\n * \code intersectionPoint = start1 + factor1 * (end1-start1) \endcode * \code intersectionPoint2 = start2 + factor2 * (end2-start2) \endcode * Lines are treated as infinite.\n * false is returned if the lines are parallel and true if they are not. * Only the XZ coordinates are used. */ public static bool IntersectionFactor (Int3 start1, Int3 end1, Int3 start2, Int3 end2, out float factor1, out float factor2) { Int3 dir1 = end1-start1; Int3 dir2 = end2-start2; long den = dir2.z*dir1.x - dir2.x * dir1.z; if (den == 0) { factor1 = 0; factor2 = 0; return false; } long nom = dir2.x*(start1.z-start2.z)- dir2.z*(start1.x-start2.x); long nom2 = dir1.x*(start1.z-start2.z) - dir1.z * (start1.x - start2.x); factor1 = (float)nom/den; factor2 = (float)nom2/den; return true; }
/** 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 (Int3 lineStart, Int3 lineEnd, Int3 point) { Int3 lineDirection = lineEnd-lineStart; float magn = lineDirection.sqrMagnitude; float closestPoint = Int3.Dot((point-lineStart),lineDirection); //Vector3.Dot(lineDirection,lineDirection); if (magn != 0) closestPoint /= magn; return closestPoint; //return closestPoint / magn; }
/** Returns if the points are colinear (lie on a straight line) */ public static bool IsColinear (Int3 a, Int3 b, Int3 c) { return (long)(b.x - a.x) * (long)(c.z - a.z) - (long)(c.x - a.x) * (long)(b.z - a.z) == 0; }
/** 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). * * \note XZ space */ public static bool Intersects (Int3 a, Int3 b, Int3 a2, Int3 b2) { return Left (a,b,a2) != Left (a,b,b2) && Left (a2,b2,a) != Left (a2,b2,b); }
/** @} */ /** Sets the start and end points. * Sets #originalStartPoint, #originalEndPoint, #startPoint, #endPoint, #startIntPoint and #hTarget (to \a end ) */ protected void UpdateStartEnd (Vector3 start, Vector3 end) { originalStartPoint = start; originalEndPoint = end; startPoint = start; endPoint = end; startIntPoint = (Int3)start; hTarget = (Int3)end; }
/** Reset all values to their default values. * All inheriting path types must implement this function, resetting ALL their variables to enable recycling of paths. * Call this base function in inheriting types with base.Reset (); */ public override void Reset () { base.Reset (); startNode = null; endNode = null; startHint = null; endHint = null; originalStartPoint = Vector3.zero; originalEndPoint = Vector3.zero; startPoint = Vector3.zero; endPoint = Vector3.zero; calculatePartial = false; partialBestTarget = null; startIntPoint = new Int3(); hTarget = new Int3(); endNodeCosts = null; }
/** Builds a polygon mesh from a contour set. * * \param cset contour set to build a mesh from. * \param nvp Maximum allowed vertices per polygon. \warning Currently locked to 3. * \param mesh Results will be written to this mesh. */ public void BuildPolyMesh (VoxelContourSet cset, int nvp, out VoxelMesh mesh) { AstarProfiler.StartProfile ("Build Poly Mesh"); nvp = 3; int maxVertices = 0; int maxTris = 0; int maxVertsPerCont = 0; for (int i = 0; i < cset.conts.Count; i++) { // Skip null contours. if (cset.conts[i].nverts < 3) continue; maxVertices += cset.conts[i].nverts; maxTris += cset.conts[i].nverts - 2; maxVertsPerCont = AstarMath.Max (maxVertsPerCont, cset.conts[i].nverts); } if (maxVertices >= 65534) { Debug.LogWarning ("To many vertices for unity to render - Unity might screw up rendering, but hopefully the navmesh will work ok"); //mesh = new VoxelMesh (); //yield break; //return; } /** \todo Could be cached to avoid allocations */ Int3[] verts = new Int3[maxVertices]; /** \todo Could be cached to avoid allocations */ int[] polys = new int[maxTris*nvp]; Memory.MemSet<int> (polys, 0xff, sizeof(int)); int[] indices = new int[maxVertsPerCont]; int[] tris = new int[maxVertsPerCont*3]; int vertexIndex = 0; int polyIndex = 0; for (int i=0;i<cset.conts.Count;i++) { VoxelContour cont = cset.conts[i]; //Skip null contours if (cont.nverts < 3) { continue; } for (int j=0; j < cont.nverts;j++) { indices[j] = j; cont.verts[j*4+2] /= voxelArea.width; } int ntris = Triangulate (cont.nverts, cont.verts, ref indices, ref tris); int startIndex = vertexIndex; for (int j=0;j<ntris*3; polyIndex++, j++) { //@Error sometimes polys[polyIndex] = tris[j]+startIndex; } for (int j=0;j<cont.nverts; vertexIndex++, j++) { verts[vertexIndex] = new Int3(cont.verts[j*4],cont.verts[j*4+1],cont.verts[j*4+2]); } } mesh = new VoxelMesh (); //yield break; Int3[] trimmedVerts = new Int3[vertexIndex]; for (int i=0;i<vertexIndex;i++) { trimmedVerts[i] = verts[i]; } int[] trimmedTris = new int[polyIndex]; System.Buffer.BlockCopy (polys, 0, trimmedTris, 0, polyIndex*sizeof(int)); mesh.verts = trimmedVerts; mesh.tris = trimmedTris; // Some debugging /*for (int i=0;i<mesh.tris.Length/3;i++) { int p = i*3; int p1 = mesh.tris[p]; int p2 = mesh.tris[p+1]; int p3 = mesh.tris[p+2]; //Debug.DrawLine (ConvertPosCorrZ (mesh.verts[p1].x,mesh.verts[p1].y,mesh.verts[p1].z),ConvertPosCorrZ (mesh.verts[p2].x,mesh.verts[p2].y,mesh.verts[p2].z),Color.yellow); //Debug.DrawLine (ConvertPosCorrZ (mesh.verts[p1].x,mesh.verts[p1].y,mesh.verts[p1].z),ConvertPosCorrZ (mesh.verts[p3].x,mesh.verts[p3].y,mesh.verts[p3].z),Color.yellow); //Debug.DrawLine (ConvertPosCorrZ (mesh.verts[p3].x,mesh.verts[p3].y,mesh.verts[p3].z),ConvertPosCorrZ (mesh.verts[p2].x,mesh.verts[p2].y,mesh.verts[p2].z),Color.yellow); //Debug.DrawLine (ConvertPosCorrZ (verts[p1],0,verts[p1+2]),ConvertPosCorrZ (verts[p2],0,verts[p2+2]),Color.blue); //Debug.DrawLine (ConvertPosCorrZ (verts[p1],0,verts[p1+2]),ConvertPosCorrZ (verts[p3],0,verts[p3+2]),Color.blue); //Debug.DrawLine (ConvertPosCorrZ (verts[p2],0,verts[p2+2]),ConvertPosCorrZ (verts[p3],0,verts[p3+2]),Color.blue); }*/ AstarProfiler.EndProfile ("Build Poly Mesh"); }
public override void DeserializeNode (GraphSerializationContext ctx) { base.DeserializeNode (ctx); position = new Int3 (ctx.reader.ReadInt32(), ctx.reader.ReadInt32(), ctx.reader.ReadInt32()); }
public static int ClipPolygon (Int3[] vIn, int n, Int3[] vOut, int multi, int offset, int axis) { int[] d = clipPolygonIntCache; for (int i=0;i<n;i++) { d[i] = multi*vIn[i][axis]+offset; } //Number of resulting vertices int m = 0; for (int i=0, j = n-1; i < n; j=i, i++) { bool prev = d[j] >= 0; bool curr = d[i] >= 0; if (prev != curr) { double s = (double)d[j] / (d[j] - d[i]); vOut[m] = vIn[j] + (vIn[i]-vIn[j])*s; m++; } if (curr) { vOut[m] = vIn[i]; m++; } } return m; }
public override void DeserializeNode (GraphSerializationContext ctx) { base.DeserializeNode (ctx); position = new Int3(ctx.reader.ReadInt32(), ctx.reader.ReadInt32(), ctx.reader.ReadInt32()); gridFlags = ctx.reader.ReadUInt16(); #if ASTAR_LEVELGRIDNODE_FEW_LAYERS gridConnections = ctx.reader.ReadUInt16(); #else gridConnections = ctx.reader.ReadUInt32(); #endif }
public void SetPosition (Int3 position) { this.position = position; }
/** Returns if the points a in a clockwise order */ public static bool IsClockwise (Int3 a, Int3 b, Int3 c) { return LeftNotColinear(a, b, c); }
/** Prepares the path. Searches for start and end nodes and does some simple checking if a path is at all possible */ public override void Prepare () { AstarProfiler.StartProfile ("Get Nearest"); //Initialize the NNConstraint nnConstraint.tags = enabledTags; NNInfo startNNInfo = AstarPath.active.GetNearest (startPoint,nnConstraint, startHint); //Tell the NNConstraint which node was found as the start node if it is a PathNNConstraint and not a normal NNConstraint var pathNNConstraint = nnConstraint as PathNNConstraint; if (pathNNConstraint != null) { pathNNConstraint.SetStart (startNNInfo.node); } startPoint = startNNInfo.clampedPosition; startIntPoint = (Int3)startPoint; startNode = startNNInfo.node; //If it is declared that this path type has an end point //Some path types might want to use most of the ABPath code, but will not have an explicit end point at this stage if (hasEndPoint) { NNInfo endNNInfo = AstarPath.active.GetNearest (endPoint,nnConstraint, endHint); endPoint = endNNInfo.clampedPosition; // Note, other methods assume hTarget is (Int3)endPoint hTarget = (Int3)endPoint; endNode = endNNInfo.node; hTargetNode = endNode; } AstarProfiler.EndProfile (); #if ASTARDEBUG if (startNode != null) Debug.DrawLine ((Vector3)startNode.position,startPoint,Color.blue); if (endNode != null) Debug.DrawLine ((Vector3)endNode.position,endPoint,Color.blue); #endif if (startNode == null && (hasEndPoint && endNode == null)) { Error (); LogError ("Couldn't find close nodes to the start point or the end point"); return; } if (startNode == null) { Error (); LogError ("Couldn't find a close node to the start point"); return; } if (endNode == null && hasEndPoint) { Error (); LogError ("Couldn't find a close node to the end point"); return; } if (!startNode.Walkable) { #if ASTARDEBUG Debug.DrawRay (startPoint,Vector3.up,Color.red); Debug.DrawLine (startPoint,(Vector3)startNode.position,Color.red); #endif Error (); LogError ("The node closest to the start point is not walkable"); return; } if (hasEndPoint && !endNode.Walkable) { Error (); LogError ("The node closest to the end point is not walkable"); return; } if (hasEndPoint && startNode.Area != endNode.Area) { Error (); LogError ("There is no valid path to the target (start area: "+startNode.Area+", target area: "+endNode.Area+")"); return; } }
/** Returns true if the points a in a clockwise order or if they are colinear */ public static bool IsClockwiseMargin (Int3 a, Int3 b, Int3 c) { return Left(a, b, c); }
/** Returns the approximate shortest squared distance between x,z and the line p-q. * The line is considered infinite. * This function is not entirely exact, but it is about twice as fast as DistancePointSegment2. */ public static float DistancePointSegment (Int3 a, Int3 b, Int3 p) { float pqx = (float)(b.x - a.x); float pqz = (float)(b.z - a.z); float dx = (float)(p.x - a.x); float dz = (float)(p.z - a.z); float d = pqx*pqx + pqz*pqz; float t = pqx*dx + pqz*dz; if (d > 0) t /= d; if (t < 0) t = 0; else if (t > 1) t = 1; dx = a.x + t*pqx - p.x; dz = a.z + t*pqz - p.z; return dx*dx + dz*dz; }
/** Returns if the points are colinear (lie on a straight line) */ public static bool IsColinearAlmost (Int3 a, Int3 b, Int3 c) { long v = (long)(b.x - a.x) * (long)(c.z - a.z) - (long)(c.x - a.x) * (long)(b.z - a.z); return v > -1 && v < 1; }
/** Signed area of a triangle in the XZ plane multiplied by 2. * This will be negative for clockwise triangles and positive for counter-clockwise ones */ public static long TriangleArea2 (Int3 a, Int3 b, Int3 c) { return (long)(b.x - a.x) * (long)(c.z - a.z) - (long)(c.x - a.x) * (long)(b.z - a.z); }
/** Returns if the ray (start1, end1) intersects the segment (start2, end2). * false is returned if the lines are parallel. * Only the XZ coordinates are used. * \todo Double check that this actually works */ public static bool IntersectionFactorRaySegment (Int3 start1, Int3 end1, Int3 start2, Int3 end2) { Int3 dir1 = end1-start1; Int3 dir2 = end2-start2; long den = dir2.z*dir1.x - dir2.x * dir1.z; if (den == 0) { return false; } long nom = dir2.x*(start1.z-start2.z)- dir2.z*(start1.x-start2.x); long nom2 = dir1.x*(start1.z-start2.z) - dir1.z * (start1.x - start2.x); //factor1 < 0 // If both have the same sign, then nom/den < 0 and thus the segment cuts the ray before the ray starts if (!(nom < 0 ^ den < 0)) { return false; } //factor2 < 0 if(!(nom2 < 0 ^ den < 0)) { return false; } if ((den >= 0 && nom2 > den) || (den < 0 && nom2 <= den)) { return false; } //factor1 = (float)nom/den; //factor2 = (float)nom2/den; return true; }
public static long TriangleArea (Int3 a, Int3 b, Int3 c) { return TriangleArea2(a, b, c); }
/** Returns the intersection factor for line 1 with ray 2. * The intersection factors is a factor distance along the line \a start - \a end where the other line intersects it.\n * \code intersectionPoint = start1 + factor * (end1-start1) \endcode * Lines are treated as infinite.\n * * The second "line" is treated as a ray, meaning only matches on start2 or forwards towards end2 (and beyond) will be returned * If the point lies on the wrong side of the ray start, Nan will be returned. * * NaN is returned if the lines are parallel. */ public static float IntersectionFactorRay (Int3 start1, Int3 end1, Int3 start2, Int3 end2) { Int3 dir1 = end1-start1; Int3 dir2 = end2-start2; int den = dir2.z*dir1.x - dir2.x * dir1.z; if (den == 0) { return float.NaN; } int nom = dir2.x*(start1.z-start2.z)- dir2.z*(start1.x-start2.x); int nom2 = dir1.x*(start1.z-start2.z) - dir1.z * (start1.x - start2.x); if ((float)nom2/den < 0) { return float.NaN; } return (float)nom/den; }
/** Returns if the triangle \a ABC contains the point \a p */ public static bool ContainsPoint (Int3 a, Int3 b, Int3 c, Int3 p) { return Polygon.IsClockwiseMargin (a,b, p) && Polygon.IsClockwiseMargin (b,c, p) && Polygon.IsClockwiseMargin (c,a, p); }
public void SetPosition (Int3 value) { position = value; }
/** Returns if \a p lies on the left side of the line \a a - \a b. Uses XZ space. Also returns true if the points are colinear */ public static bool Left (Int3 a, Int3 b, Int3 c) { return (long)(b.x - a.x) * (long)(c.z - a.z) - (long)(c.x - a.x) * (long)(b.z - a.z) <= 0; }
public void GetMesh ( Int3 offset, ref Int3[] vbuffer, out int[] tbuffer ) { if ( verts == null ) RebuildMesh (); if ( verts == null ) { tbuffer = new int[0]; return; } if ( vbuffer == null || vbuffer.Length < verts.Length ) vbuffer = new Int3[verts.Length]; tbuffer = tris; if ( useRotation ) { Matrix4x4 m = Matrix4x4.TRS ( tr.position + center, tr.rotation, tr.localScale * meshScale ); for ( int i=0;i<verts.Length;i++) { vbuffer[i] = offset + (Int3)m.MultiplyPoint3x4 ( verts[i] ); } } else { Vector3 voffset = tr.position + center; for ( int i=0;i<verts.Length;i++) { vbuffer[i] = offset + (Int3)(voffset + verts[i]*meshScale); } } }
/** Returns if \a p lies on the left side of the line \a a - \a b. Uses XZ space. */ public static bool LeftNotColinear (Int3 a, Int3 b, Int3 c) { return (long)(b.x - a.x) * (long)(c.z - a.z) - (long)(c.x - a.x) * (long)(b.z - a.z) < 0; }
public void SetPosition (Int3 p) { position = p; }
public static long DotLong (Int3 lhs, Int3 rhs) { return (long)lhs.x * (long)rhs.x + (long)lhs.y * (long)rhs.y + (long)lhs.z * (long)rhs.z; }