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