/// <summary> /// The main entry point for the application. /// </summary> static void Main(string[] args) { using (Pathfinding game = new Pathfinding()) { game.Run(); } }
/** Deserializes common info additively * Common info is what is shared between the editor serialization and the runtime serializer. * This is mostly everything except the graph inspectors which serialize some extra data in the editor */ public void DeserializeGraphsPartAdditive(Pathfinding.Serialization.AstarSerializer sr) { if (graphs == null) graphs = new NavGraph[0]; if (userConnections == null) userConnections = new UserConnection[0]; List<NavGraph> gr = new List<NavGraph>(graphs); gr.AddRange (sr.DeserializeGraphs ()); graphs = gr.ToArray(); List<UserConnection> conns = new List<UserConnection>(userConnections); conns.AddRange (sr.DeserializeUserConnections()); userConnections = conns.ToArray (); sr.DeserializeNodes(); sr.DeserializeExtraInfo(); sr.PostDeserialization(); for (int i=0;i<graphs.Length;i++) { for (int j=i+1;j<graphs.Length;j++) { if (graphs[i].guid == graphs[j].guid) { Debug.LogWarning ("Guid Conflict when importing graphs additively. Imported graph will get a new Guid.\nThis message is (relatively) harmless."); graphs[i].guid = Pathfinding.Util.Guid.NewGuid (); break; } } } }
/** Deserializes common info. * Common info is what is shared between the editor serialization and the runtime serializer. * This is mostly everything except the graph inspectors which serialize some extra data in the editor */ public void DeserializeGraphsPart (Pathfinding.Serialization.AstarSerializer sr) { ClearGraphs (); graphs = sr.DeserializeGraphs (); if ( graphs != null ) for ( int i = 0; i<graphs.Length;i++ ) if ( graphs[i] != null ) graphs[i].graphIndex = (uint)i; userConnections = sr.DeserializeUserConnections(); //sr.DeserializeNodes(); sr.DeserializeExtraInfo(); //Assign correct graph indices. for (int i=0;i<graphs.Length;i++) { if (graphs[i] == null) continue; graphs[i].GetNodes (delegate (GraphNode node) { node.GraphIndex = (uint)i; return true; }); } sr.PostDeserialization(); }
public float DistanceTo(Pathfinding.IPoint pPoint) { if (pPoint is TileNode) { TileNode otherNode = pPoint as TileNode; return this.worldPosition.DistanceTo(otherNode.worldPosition); } else { throw new NotImplementedException(); } }
/** Deserializes common info. * Common info is what is shared between the editor serialization and the runtime serializer. * This is mostly everything except the graph inspectors which serialize some extra data in the editor */ public void DeserializeGraphsPart (Pathfinding.Serialization.AstarSerializer sr) { ClearGraphs (); graphs = sr.DeserializeGraphs (); if ( graphs != null ) for ( int i = 0; i<graphs.Length;i++ ) if ( graphs[i] != null ) graphs[i].graphIndex = (uint)i; userConnections = sr.DeserializeUserConnections(); //sr.DeserializeNodes(); sr.DeserializeExtraInfo(); sr.PostDeserialization(); }
/** Main serializer function. * Serializes all graphs to a byte array * A similar function exists in the AstarEditor.cs script to save additional info */ public byte[] SerializeGraphs (Pathfinding.Serialization.SerializeSettings settings, out uint checksum) { AstarPath.active.BlockUntilPathQueueBlocked(); Pathfinding.Serialization.AstarSerializer sr = new Pathfinding.Serialization.AstarSerializer(this, settings); sr.OpenSerialize(); SerializeGraphsPart (sr); byte[] bytes = sr.CloseSerialize(); checksum = sr.GetChecksum (); #if ASTARDEBUG Debug.Log ("Got a whole bunch of data, "+bytes.Length+" bytes"); #endif return bytes; }
public void SaveCacheData (Pathfinding.Serialization.SerializeSettings settings) { data_cachedStartup = SerializeGraphs (settings); cacheStartup = true; }
/** Deserializes common info additively * Common info is what is shared between the editor serialization and the runtime serializer. * This is mostly everything except the graph inspectors which serialize some extra data in the editor */ public void DeserializeGraphsPartAdditive(Pathfinding.Serialize.AstarSerializer sr) { if (graphs == null) graphs = new NavGraph[0]; if (userConnections == null) userConnections = new UserConnection[0]; List<NavGraph> gr = new List<NavGraph>(graphs); gr.AddRange (sr.DeserializeGraphs ()); graphs = gr.ToArray(); List<UserConnection> conns = new List<UserConnection>(userConnections); conns.AddRange (sr.DeserializeUserConnections()); userConnections = conns.ToArray (); sr.DeserializeNodes(); sr.DeserializeExtraInfo(); sr.PostDeserialization(); }
/** Deserializes common info. * Common info is what is shared between the editor serialization and the runtime serializer. * This is mostly everything except the graph inspectors which serialize some extra data in the editor */ public void DeserializeGraphsPart (Pathfinding.Serialization.AstarSerializer sr) { ClearGraphs (); graphs = sr.DeserializeGraphs (); sr.DeserializeExtraInfo(); //Assign correct graph indices. for (int i=0;i<graphs.Length;i++) { if (graphs[i] == null) continue; graphs[i].GetNodes (node => { node.GraphIndex = (uint)i; return true; }); } sr.PostDeserialization(); }
/** Main serializer function. * Serializes all graphs to a byte array * A similar function exists in the AstarPathEditor.cs script to save additional info */ public byte[] SerializeGraphs (Pathfinding.Serialization.SerializeSettings settings, out uint checksum) { AstarPath.active.BlockUntilPathQueueBlocked(); var sr = new Pathfinding.Serialization.AstarSerializer(this, settings); sr.OpenSerialize(); SerializeGraphsPart (sr); byte[] bytes = sr.CloseSerialize(); checksum = sr.GetChecksum (); return bytes; }
/** Adds obstacles for a graph */ public void AddGraphObstacles (Pathfinding.RVO.Simulator sim, NavGraph graph) { if (obstacles.Count > 0 && lastSim != null && lastSim != sim) { Debug.LogError("Simulator has changed but some old obstacles are still added for the previous simulator. Deleting previous obstacles."); RemoveObstacles(); } //Remember which simulator these obstacles were added to lastSim = sim; var ng = graph as INavmesh; if (ng == null) return; //Assume less than 20 vertices per node (actually assumes 3, but I will change that some day) var uses = new int[20]; ng.GetNodes(delegate(GraphNode _node) { var node = _node as TriangleMeshNode; uses[0] = uses[1] = uses[2] = 0; if (node != null) { //Find out which edges are shared with other nodes for (int j = 0; j < node.connections.Length; j++) { var other = node.connections[j] as TriangleMeshNode; // Not necessarily a TriangleMeshNode if (other != null) { int a = node.SharedEdge(other); if (a != -1) uses[a] = 1; } } //Loop through all edges on the node for (int j = 0; j < 3; j++) { //The edge is not shared with any other node //I.e it is an exterior edge on the mesh if (uses[j] == 0) { //The two vertices of the edge var v1 = (Vector3)node.GetVertex(j); var v2 = (Vector3)node.GetVertex((j+1) % node.GetVertexCount()); //I think node vertices always should be clockwise, but it's good to be certain /*if (!Polygon.IsClockwise (v1,v2,(Vector3)node.GetVertex((j+2) % node.GetVertexCount()))) { * Vector3 tmp = v2; * v2 = v1; * v1 = tmp; * }*/ #if ASTARDEBUG Debug.DrawLine(v1, v2, Color.red); Debug.DrawRay(v1, Vector3.up*wallHeight, Color.red); #endif //Find out the height of the wall/obstacle we are about to add float height = System.Math.Abs(v1.y-v2.y); height = System.Math.Max(height, 5); //Add the edge as a line obstacle obstacles.Add(sim.AddObstacle(v1, v2, wallHeight)); } } } return true; }); }
void DrawIsometricField(GridGraph graph) { var isometricGUIContent = new GUIContent ("Isometric Angle", "For an isometric 2D game, you can use this parameter to scale the graph correctly.\nIt can also be used to create a hexagon grid."); var isometricOptions = new [] {new GUIContent ("None (0°)"), new GUIContent ("Isometric (≈54.74°)"), new GUIContent("Custom")}; var isometricValues = new [] {0f, standardIsometric}; var isometricOption = 2; for (int i = 0; i < isometricValues.Length; i++) { if (Mathf.Approximately (graph.isometricAngle, isometricValues[i])) { isometricOption = i; } } var prevIsometricOption = isometricOption; isometricOption = EditorGUILayout.IntPopup (isometricGUIContent, isometricOption, isometricOptions, new [] {0, 1, 2}); if (prevIsometricOption != isometricOption) { // Change to something that will not match the predefined values above graph.isometricAngle = 45; } if (isometricOption < 2) { graph.isometricAngle = isometricValues[isometricOption]; } else { // Custom graph.isometricAngle = EditorGUILayout.FloatField (isometricGUIContent, graph.isometricAngle); } }
/** Serializes common info to the serializer. * Common info is what is shared between the editor serialization and the runtime serializer. * This is mostly everything except the graph inspectors which serialize some extra data in the editor */ public void SerializeGraphsPart (Pathfinding.Serialization.AstarSerializer sr) { sr.SerializeGraphs(graphs); sr.SerializeExtraInfo(); }
internal void CalculateVelocity ( Pathfinding.RVO.Simulator.WorkerContext context ) { if ( locked ) { newVelocity = Vector2.zero; return; } if ( context.vos.Length < neighbours.Count ) { context.vos = new VO[Mathf.Max(context.vos.Length*2, neighbours.Count)]; } Vector2 position2D = new Vector2(position.x,position.z); var vos = context.vos; var voCount = 0; Vector2 optimalVelocity = new Vector2(velocity.x, velocity.z); float inverseAgentTimeHorizon = 1.0f/agentTimeHorizon; for ( int o = 0; o < neighbours.Count; o++ ) { Agent other = neighbours[o]; if ( other == this ) continue; float maxY = System.Math.Min (position.y+height,other.position.y+other.height); float minY = System.Math.Max (position.y,other.position.y); //The agents cannot collide since they //are on different y-levels if (maxY - minY < 0) { continue; } Vector2 otherOptimalVelocity = new Vector2(other.Velocity.x, other.velocity.z); float totalRadius = radius + other.radius; // Describes a circle on the border of the VO //float boundingRadius = totalRadius * inverseAgentTimeHorizon; Vector2 voBoundingOrigin = new Vector2(other.position.x,other.position.z) - position2D; //float boundingDist = voBoundingOrigin.magnitude; Vector2 relativeVelocity = optimalVelocity - otherOptimalVelocity; { //voBoundingOrigin *= inverseAgentTimeHorizon; //boundingDist *= inverseAgentTimeHorizon; // Common case, no collision vos[voCount] = new VO( voBoundingOrigin, (optimalVelocity + otherOptimalVelocity)*0.5f, totalRadius, relativeVelocity, inverseAgentTimeHorizon); voCount++; } #if OLD //Debug.Log (boundingDist + " " + total if ( boundingDist < totalRadius ) { // Collision // We want the collision to end this frame, so ignore the inverseAgentTimeHorizon and // instead use the frame's delta time instead. The closest point will not always be // on the circle, but the velocity will have to be really // high for it not to be, so just assume the velocity is low // (if not, the agents will jump through each other) voBoundingOrigin *= (1.0f/simulator.DeltaTime); boundingDist *= (1.0f/simulator.DeltaTime); Vector2 d = relativeVelocity - voBoundingOrigin; float dm = d.magnitude; // Normalize d /= dm; float resMagn = (totalRadius*(1.0f/simulator.DeltaTime) - dm); maxRelVelMagn = System.Math.Max ( resMagn, maxRelVelMagn ); Vector2 closestOnCircleDelta = d*resMagn;//d*(boundingDist-dm); Vector2 closestOnCircleVector = d; halfPlanes.Add ( new HalfPlane ( closestOnCircleDelta*0.5f + optimalVelocity, closestOnCircleVector ) ); if (debug) halfPlanes[halfPlanes.Count-1].Draw ( Color.blue ); } else { voBoundingOrigin *= inverseAgentTimeHorizon; boundingDist *= inverseAgentTimeHorizon; // Common case, no collision VO vo = new VO( voBoundingOrigin + (optimalVelocity + otherOptimalVelocity)*0.5f, voBoundingOrigin.normalized, totalRadius*inverseAgentTimeHorizon); maxRelVelMagn = System.Math.Max ( maxRelVelMagn, relativeVelocity.magnitude ); // Optimized: float factor = Vector3.Dot ( optimalVelocity, voBoundingOrigin ) < (boundingDist^2); if (debug) DrawVO ( voBoundingOrigin, boundingRadius, Vector2.zero); if ( debug ) { //Debug.DrawLine ( To3D(position), To3D ( relativeVelocity ), Color.blue ); DrawCross ( relativeVelocity, Color.white, 0.5f); } //DrawCross ( voBoundingOrigin ); float alpha = Mathf.Atan2 ( -voBoundingOrigin.y, -voBoundingOrigin.x ); float delta = Mathf.Abs(Mathf.Acos (boundingRadius/(voBoundingOrigin).magnitude)); float actualDot = Vector2.Dot ( -voBoundingOrigin.normalized, (relativeVelocity-voBoundingOrigin).normalized ); float dot = Mathf.Abs ( Mathf.Acos ( Mathf.Clamp01(actualDot) ) ); bool closestIsCircle = dot < delta; /*DrawCircle ( circleCenter, radius, alpha-delta, alpha+delta, Color.black ); Vector2 p1 = circleCenter + new Vector2 ( Mathf.Cos (alpha-delta), Mathf.Sin (alpha-delta) ) * boundingRadius; Vector2 p2 = circleCenter + new Vector2 ( Mathf.Cos (alpha+delta), Mathf.Sin (alpha+delta) ) * boundingRadius;*/ if ( debug ) Debug.Log (" <= 1 && " + dot + " < " + delta + " ( " +actualDot + ")"); if ( closestIsCircle ) { // The cutoff circle is the closest // part of the VO // Find the closest point on the VO Vector2 closestOnCircleVector = (relativeVelocity - voBoundingOrigin); float magn = closestOnCircleVector.magnitude; closestOnCircleVector = closestOnCircleVector/magn;//(closestOnCircleVector/magn)*boundingRadius*(1-(magn/boundingRadius)); Vector2 closestOnCircle = closestOnCircleVector*boundingRadius + voBoundingOrigin; //if ( magn > boundingRadius ) closestOnCircleVector = -closestOnCircleVector; if ( debug ) DrawCross ( closestOnCircle, 1); halfPlanes.Add ( new HalfPlane ( (closestOnCircle - relativeVelocity)*0.5f + optimalVelocity, closestOnCircleVector.normalized ) ); if (debug) halfPlanes[halfPlanes.Count-1].Draw ( closestIsCircle ? Color.blue : Color.red ); } else { // One of the legs of the VO is the closest part // find the closest point on the VO bool left = Polygon.Left ( Vector2.zero, voBoundingOrigin, relativeVelocity ); float gamma = alpha + (left ? delta : -delta); // Point on circle Vector2 line = new Vector2 ( Mathf.Cos (gamma), Mathf.Sin (gamma) ) * boundingRadius; // Vector tangent to circle which is the correct line tangent Vector2 dir = new Vector2(line.y,-line.x).normalized; // Point in space line += voBoundingOrigin; Vector2 onLine = line + dir * Vector2.Dot ( dir, relativeVelocity - line); if (debug) { DrawCross ( onLine, Color.red, 1 ); Debug.DrawRay ( To3D(line), To3D(dir*10), Color.red ); Debug.Log (line + " " + dir + " " + gamma + " " + relativeVelocity); } if ( !left ) dir = -dir; halfPlanes.Add ( new HalfPlane ( (onLine - relativeVelocity)*0.5f + optimalVelocity, (line-voBoundingOrigin).normalized ) ); if (debug) halfPlanes[halfPlanes.Count-1].Draw ( Color.blue ); } } #endif } if ( this.DebugDraw ) { /*for ( int x = 0; x < simulator.tex.width; x++ ) { for ( int y = 0; y < simulator.tex.height; y++ ) { Vector2 p = new Vector2 (x*simulator.textureSize / simulator.tex.width, y*simulator.textureSize / simulator.tex.height); Vector2 dir = Vector2.zero; float weight = 0; for ( int i = 0; i < voCount; i++ ) { float w = 0; dir += vos[i].Sample (p-position, out w); if ( w > weight ) weight = w; } Vector2 d2 = (desiredVelocity - (p-position)); dir += d2*DesiredVelocityScale; if ( d2.magnitude * DesiredVelocityWeight > weight ) weight = d2.magnitude * DesiredVelocityWeight; if ( weight > 0 ) dir /= weight; Vector2 d3 = simulator.SampleDensity (p+position); Debug.DrawRay ( To3D(p), To3D(d3*1f), Color.blue ); simulator.Plot (p, Rainbow(weight*simulator.colorScale)); } }*/ } //if ( debug ) { float best = float.PositiveInfinity; Vector2 result = Vector2.zero; float cutoff = new Vector2(velocity.x,velocity.z).magnitude*simulator.qualityCutoff; //for ( int i = 0; i < 10; i++ ) { { result = Trace ( vos, voCount, new Vector2(desiredVelocity.x, desiredVelocity.z), cutoff, out best ); } // Can be uncommented for higher quality local avoidance /*for ( int i = 0; i < 3; i++ ) { Vector2 p = desiredVelocity + new Vector2(Mathf.Cos(Mathf.PI*2*(i/3.0f)), Mathf.Sin(Mathf.PI*2*(i/3.0f))); float score; Vector2 res = Trace ( vos, voCount, p, velocity.magnitude*simulator.qualityCutoff, out score ); if ( score < best ) { //if ( score < best*0.9f ) Debug.Log ("Better " + score + " < " + best); result = res; best = score; } }*/ { Vector2 p = Vector2.zero; float score; Vector2 res = Trace ( vos, voCount, p, cutoff, out score ); if ( score < best ) { //if ( score < best*0.9f ) Debug.Log ("Better " + score + " < " + best); result = res; best = score; } } //} if (DebugDraw) DrawCross (result+position2D); newVelocity = To3D(Vector2.ClampMagnitude (result, maxSpeed)); }
public virtual float DistanceTo(Pathfinding.IPoint pPoint) { if (pPoint is TileNode) { TileNode otherNode = pPoint as TileNode; return localPoint.EuclidianDistanceTo(otherNode.localPoint); } else { throw new NotImplementedException(); } }
/** Deserializes common info. * Common info is what is shared between the editor serialization and the runtime serializer. * This is mostly everything except the graph inspectors which serialize some extra data in the editor */ public void DeserializeGraphsPart(Pathfinding.Serialize.AstarSerializer sr) { graphs = sr.DeserializeGraphs (); userConnections = sr.DeserializeUserConnections(); sr.DeserializeNodes(); sr.DeserializeExtraInfo(); sr.PostDeserialization(); }
/** Deserializes common info additively * Common info is what is shared between the editor serialization and the runtime serializer. * This is mostly everything except the graph inspectors which serialize some extra data in the editor */ public void DeserializeGraphsPartAdditive (Pathfinding.Serialization.AstarSerializer sr) { if (graphs == null) graphs = new NavGraph[0]; var gr = new List<NavGraph>(graphs); // Set an offset so that the deserializer will load // the graphs with the correct graph indexes sr.SetGraphIndexOffset (gr.Count); gr.AddRange (sr.DeserializeGraphs ()); graphs = gr.ToArray(); //Assign correct graph indices. Issue #21 for (int i=0;i<graphs.Length;i++) { if (graphs[i] == null) continue; graphs[i].GetNodes (node => { node.GraphIndex = (uint)i; return true; }); } sr.DeserializeExtraInfo(); sr.PostDeserialization(); for (int i=0;i<graphs.Length;i++) { for (int j=i+1;j<graphs.Length;j++) { if (graphs[i] != null && graphs[j] != null && graphs[i].guid == graphs[j].guid) { Debug.LogWarning ("Guid Conflict when importing graphs additively. Imported graph will get a new Guid.\nThis message is (relatively) harmless."); graphs[i].guid = Pathfinding.Util.Guid.NewGuid (); break; } } } }
/** Main serializer function. * Serializes all graphs to a byte array * A similar function exists in the AstarEditor.cs script to save additional info */ public byte[] SerializeGraphs(Pathfinding.Serialize.SerializeSettings settings, out uint checksum) { Pathfinding.Serialize.AstarSerializer sr = new Pathfinding.Serialize.AstarSerializer(this, settings); sr.OpenSerialize(); SerializeGraphsPart (sr); byte[] bytes = sr.CloseSerialize(); checksum = sr.GetChecksum (); return bytes; }
public Vector3 IntPointToV3 (Pathfinding.ClipperLib.IntPoint p) { Int3 ip = new Int3((int)p.X,0,(int)p.Y); return (Vector3)ip; }
/** Main serializer function. */ public byte[] SerializeGraphs (Pathfinding.Serialization.SerializeSettings settings) { uint checksum; return SerializeGraphs (settings, out checksum); }
internal void CalculateVelocity(Pathfinding.RVO.Simulator.WorkerContext context) { if (locked) { newVelocity = Vector2.zero; return; } if (context.vos.Length < neighbours.Count + simulator.obstacles.Count) { context.vos = new VO[Mathf.Max(context.vos.Length * 2, neighbours.Count + simulator.obstacles.Count)]; } Vector2 position2D = new Vector2(position.x, position.z); var vos = context.vos; var voCount = 0; Vector2 optimalVelocity = new Vector2(velocity.x, velocity.z); float inverseAgentTimeHorizon = 1.0f / agentTimeHorizon; float wallThickness = simulator.WallThickness; float wallWeight = simulator.algorithm == Simulator.SamplingAlgorithm.GradientDecent ? 1 : WallWeight; for (int i = 0; i < simulator.obstacles.Count; i++) { var obstacle = simulator.obstacles[i]; var vertex = obstacle; do { if (vertex.ignore || position.y > vertex.position.y + vertex.height || position.y + height < vertex.position.y || (vertex.layer & collidesWith) == 0) { vertex = vertex.next; Debug.Log("continue"); continue; } float cross = VO.Det(new Vector2(vertex.position.x, vertex.position.z), vertex.dir, position2D);// vertex.dir.x * ( vertex.position.z - position.z ) - vertex.dir.y * ( vertex.position.x - position.x ); // Signed distance from the line (not segment), lines are infinite // Usually divided by vertex.dir.magnitude, but that is known to be 1 float signedDist = cross; float dotFactor = Vector2.Dot(vertex.dir, position2D - new Vector2(vertex.position.x, vertex.position.z)); // It is closest to the segment // if the dotFactor is <= 0 or >= length of the segment // WallThickness*0.1 is added as a margin to avoid false positives when moving along the edges of square obstacles bool closestIsEndpoints = dotFactor <= wallThickness * 0.05f || dotFactor >= (new Vector2(vertex.position.x, vertex.position.z) - new Vector2(vertex.next.position.x, vertex.next.position.z)).magnitude - wallThickness * 0.05f; if (Mathf.Abs(signedDist) < neighbourDist) { if (signedDist <= 0 && !closestIsEndpoints && signedDist > -wallThickness) { // Inside the wall on the "wrong" side vos[voCount] = new VO(position2D, new Vector2(vertex.position.x, vertex.position.z) - position2D, vertex.dir, wallWeight * 2); voCount++; } else if (signedDist > 0) { //Debug.DrawLine (position, (vertex.position+vertex.next.position)*0.5f, Color.yellow); Vector2 p1 = new Vector2(vertex.position.x, vertex.position.z) - position2D; Vector2 p2 = new Vector2(vertex.next.position.x, vertex.next.position.z) - position2D; Vector2 tang1 = (p1).normalized; Vector2 tang2 = (p2).normalized; vos[voCount] = new VO(position2D, p1, p2, tang1, tang2, wallWeight); voCount++; } } vertex = vertex.next; } while (vertex != obstacle); } for (int o = 0; o < neighbours.Count; o++) { Agent other = neighbours[o]; if (other == this) continue; float maxY = System.Math.Min(position.y + height, other.position.y + other.height); float minY = System.Math.Max(position.y, other.position.y); //The agents cannot collide since they //are on different y-levels if (maxY - minY < 0) { continue; } Vector2 otherOptimalVelocity = new Vector2(other.Velocity.x, other.velocity.z); float totalRadius = radius + other.radius; // Describes a circle on the border of the VO //float boundingRadius = totalRadius * inverseAgentTimeHorizon; Vector2 voBoundingOrigin = new Vector2(other.position.x, other.position.z) - position2D; //float boundingDist = voBoundingOrigin.magnitude; Vector2 relativeVelocity = optimalVelocity - otherOptimalVelocity; { //voBoundingOrigin *= inverseAgentTimeHorizon; //boundingDist *= inverseAgentTimeHorizon; // Common case, no collision Vector2 voCenter; if (other.locked) { voCenter = otherOptimalVelocity; } else { voCenter = (optimalVelocity + otherOptimalVelocity) * 0.5f; } vos[voCount] = new VO(voBoundingOrigin, voCenter, totalRadius, relativeVelocity, inverseAgentTimeHorizon, 1); voCount++; if (DebugDraw) DrawVO(position2D + voBoundingOrigin * inverseAgentTimeHorizon + voCenter, totalRadius * inverseAgentTimeHorizon, position2D + voCenter); } } Vector2 result = Vector2.zero; if (simulator.algorithm == Simulator.SamplingAlgorithm.GradientDecent) { if (this.DebugDraw) { const int PlotWidth = 40; const float WorldPlotWidth = 15; for (int x = 0; x < PlotWidth; x++) { for (int y = 0; y < PlotWidth; y++) { Vector2 p = new Vector2(x * WorldPlotWidth / PlotWidth, y * WorldPlotWidth / PlotWidth); Vector2 dir = Vector2.zero; float weight = 0; for (int i = 0; i < voCount; i++) { float w = 0; dir += vos[i].Sample(p - position2D, out w); if (w > weight) weight = w; } Vector2 d2 = (new Vector2(desiredVelocity.x, desiredVelocity.z) - (p - position2D)); dir += d2 * DesiredVelocityScale; if (d2.magnitude * DesiredVelocityWeight > weight) weight = d2.magnitude * DesiredVelocityWeight; if (weight > 0) dir /= weight; //Vector2 d3 = simulator.SampleDensity (p+position2D); Debug.DrawRay(To3D(p), To3D(d2 * 0.00f), Color.blue); //simulator.Plot (p, Rainbow(weight*simulator.colorScale)); float sc = 0; Vector2 p0 = p - Vector2.one * WorldPlotWidth * 0.5f; Vector2 p1 = Trace(vos, voCount, p0, 0.01f, out sc); if ((p0 - p1).sqrMagnitude < Sqr(WorldPlotWidth / PlotWidth) * 2.6f) { Debug.DrawRay(To3D(p1 + position2D), Vector3.up * 1, Color.red); } } } } //if ( debug ) { float best = float.PositiveInfinity; float cutoff = new Vector2(velocity.x, velocity.z).magnitude * simulator.qualityCutoff; //for ( int i = 0; i < 10; i++ ) { { result = Trace(vos, voCount, new Vector2(desiredVelocity.x, desiredVelocity.z), cutoff, out best); if (DebugDraw) DrawCross(result + position2D, Color.yellow, 0.5f); } // Can be uncommented for higher quality local avoidance /*for ( int i = 0; i < 3; i++ ) { Vector2 p = desiredVelocity + new Vector2(Mathf.Cos(Mathf.PI*2*(i/3.0f)), Mathf.Sin(Mathf.PI*2*(i/3.0f))); float score; Vector2 res = Trace ( vos, voCount, p, velocity.magnitude*simulator.qualityCutoff, out score ); if ( score < best ) { //if ( score < best*0.9f ) Debug.Log ("Better " + score + " < " + best); result = res; best = score; } }*/ { Vector2 p = this.Velocity; float score; Vector2 res = Trace(vos, voCount, p, cutoff, out score); if (score < best) { //if ( score < best*0.9f ) Debug.Log ("Better " + score + " < " + best); result = res; best = score; } if (DebugDraw) DrawCross(res + position2D, Color.magenta, 0.5f); } } else { // Adaptive sampling Vector2[] samplePos = context.samplePos; float[] sampleSize = context.sampleSize; int samplePosCount = 0; Vector2 desired2D = new Vector2(desiredVelocity.x, desiredVelocity.z); float sampleScale = Mathf.Max(radius, Mathf.Max(desired2D.magnitude, Velocity.magnitude)); samplePos[samplePosCount] = desired2D; sampleSize[samplePosCount] = sampleScale * 0.3f; samplePosCount++; const float GridScale = 0.3f; // Initial 9 samples samplePos[samplePosCount] = optimalVelocity; sampleSize[samplePosCount] = sampleScale * GridScale; samplePosCount++; { Vector2 fw = optimalVelocity * 0.5f; Vector2 rw = new Vector2(fw.y, -fw.x); const int Steps = 8; for (int i = 0; i < Steps; i++) { samplePos[samplePosCount] = rw * Mathf.Sin(i * Mathf.PI * 2 / Steps) + fw * (1 + Mathf.Cos(i * Mathf.PI * 2 / Steps)); sampleSize[samplePosCount] = (1.0f - (Mathf.Abs(i - Steps * 0.5f) / Steps)) * sampleScale * 0.5f; samplePosCount++; } const float InnerScale = 0.6f; fw *= InnerScale; rw *= InnerScale; const int Steps2 = 6; for (int i = 0; i < Steps2; i++) { samplePos[samplePosCount] = rw * Mathf.Cos((i + 0.5f) * Mathf.PI * 2 / Steps2) + fw * ((1.0f / InnerScale) + Mathf.Sin((i + 0.5f) * Mathf.PI * 2 / Steps2)); sampleSize[samplePosCount] = sampleScale * 0.3f; samplePosCount++; } const float TargetScale = 0.2f; const int Steps3 = 6; for (int i = 0; i < Steps3; i++) { samplePos[samplePosCount] = optimalVelocity + new Vector2(sampleScale * TargetScale * Mathf.Cos((i + 0.5f) * Mathf.PI * 2 / Steps3), sampleScale * TargetScale * Mathf.Sin((i + 0.5f) * Mathf.PI * 2 / Steps3)); sampleSize[samplePosCount] = sampleScale * TargetScale * 2; samplePosCount++; } } samplePos[samplePosCount] = optimalVelocity * 0.5f; sampleSize[samplePosCount] = sampleScale * 0.4f; samplePosCount++; const int KeepCount = Simulator.WorkerContext.KeepCount; Vector2[] bestPos = context.bestPos; float[] bestSizes = context.bestSizes; float[] bestScores = context.bestScores; for (int i = 0; i < KeepCount; i++) { bestScores[i] = float.PositiveInfinity; } bestScores[KeepCount] = float.NegativeInfinity; Vector2 bestEver = optimalVelocity; float bestEverScore = float.PositiveInfinity; for (int sub = 0; sub < 3; sub++) { for (int i = 0; i < samplePosCount; i++) { float score = 0; for (int vo = 0; vo < voCount; vo++) { score = System.Math.Max(score, vos[vo].ScalarSample(samplePos[i])); } // Note that velocity is a vector and speed is a scalar, not the same thing float bonusForDesiredVelocity = (samplePos[i] - desired2D).magnitude; // This didn't work out as well as I though // Code left here because I might reenable it later //float bonusForDesiredSpeed = Mathf.Abs (samplePos[i].magnitude - desired2D.magnitude); float biasedScore = score + bonusForDesiredVelocity * DesiredVelocityWeight;// + bonusForDesiredSpeed*0; score += bonusForDesiredVelocity * 0.001f; if (DebugDraw) { DrawCross(position2D + samplePos[i], Rainbow(Mathf.Log(score + 1) * 5), sampleSize[i] * 0.5f); } if (biasedScore < bestScores[0]) { for (int j = 0; j < KeepCount; j++) { if (biasedScore >= bestScores[j + 1]) { bestScores[j] = biasedScore; bestSizes[j] = sampleSize[i]; bestPos[j] = samplePos[i]; break; } } } if (score < bestEverScore) { bestEver = samplePos[i]; bestEverScore = score; if (score == 0) { sub = 100; break; } } } samplePosCount = 0; for (int i = 0; i < KeepCount; i++) { Vector2 p = bestPos[i]; float s = bestSizes[i]; bestScores[i] = float.PositiveInfinity; const float Half = 0.6f; float offset = s * Half * 0.5f; samplePos[samplePosCount + 0] = (p + new Vector2(+offset, +offset)); samplePos[samplePosCount + 1] = (p + new Vector2(-offset, +offset)); samplePos[samplePosCount + 2] = (p + new Vector2(-offset, -offset)); samplePos[samplePosCount + 3] = (p + new Vector2(+offset, -offset)); s *= s * Half; sampleSize[samplePosCount + 0] = (s); sampleSize[samplePosCount + 1] = (s); sampleSize[samplePosCount + 2] = (s); sampleSize[samplePosCount + 3] = (s); samplePosCount += 4; } } result = bestEver; } if (DebugDraw) DrawCross(result + position2D); newVelocity = To3D(Vector2.ClampMagnitude(result, maxSpeed)); if (newVelocity != Vector3.zero) { float angle = Vector3.Angle(DesiredVelocity, newVelocity); //:限制新速度方向 //如果计算出来的新速度是与设计速度的夹角大于angleMax //则新速度旋转到最大夹角 float angleMax = 45; if (angle > angleMax) { Vector2 firstObject = new Vector2(DesiredVelocity.x, DesiredVelocity.z); Vector2 secondObject = new Vector2(newVelocity.x, newVelocity.z); float det = firstObject.x * secondObject.y - firstObject.y * secondObject.x; if (det > 0)//param2 is unclockwise { Vector2 verticalObject = secondObject.Rotate(-(angle - angleMax)); newVelocity.x = verticalObject.x; newVelocity.z = verticalObject.y; if (newVelocity.magnitude < minSpeed) { newVelocity = newVelocity.normalized * minSpeed; } } else if (det < 0) { Vector2 verticalObject = secondObject.Rotate((angle - angleMax)); newVelocity.x = verticalObject.x; newVelocity.z = verticalObject.y; if (newVelocity.magnitude < minSpeed) { newVelocity = newVelocity.normalized * minSpeed; } } else { newVelocity = Vector3.zero; } } if (printVelocity) { Debug.Log("vel " + velocity + "newVelocity " + newVelocity); } } }
/** Serializes common info to the serializer. * Common info is what is shared between the editor serialization and the runtime serializer. * This is mostly everything except the graph inspectors which serialize some extra data in the editor */ public void SerializeGraphsPart (Pathfinding.Serialization.AstarSerializer sr) { sr.SerializeGraphs(graphs); sr.SerializeUserConnections (userConnections); sr.SerializeNodes(); sr.SerializeExtraInfo(); }
/** Main serializer function. * Serializes all graphs to a byte array * A similar function exists in the AstarEditor.cs script to save additional info */ public byte[] SerializeGraphs(Pathfinding.Serialize.SerializeSettings settings, out uint checksum) { Pathfinding.Serialize.AstarSerializer sr = new Pathfinding.Serialize.AstarSerializer(this, settings); sr.OpenSerialize(); SerializeGraphsPart (sr); byte[] bytes = sr.CloseSerialize(); checksum = sr.GetChecksum (); #if DEBUG Debug.Log ("Got a whole bunch of data, "+bytes.Length+" bytes"); #endif return bytes; }
/** Deserializes common info additively * Common info is what is shared between the editor serialization and the runtime serializer. * This is mostly everything except the graph inspectors which serialize some extra data in the editor */ public void DeserializeGraphsPartAdditive (Pathfinding.Serialization.AstarSerializer sr) { if (graphs == null) graphs = new NavGraph[0]; if (userConnections == null) userConnections = new UserConnection[0]; List<NavGraph> gr = new List<NavGraph>(graphs); gr.AddRange (sr.DeserializeGraphs ()); graphs = gr.ToArray(); if ( graphs != null ) for ( int i = 0; i<graphs.Length;i++ ) if ( graphs[i] != null ) graphs[i].graphIndex = (uint)i; List<UserConnection> conns = new List<UserConnection>(userConnections); conns.AddRange (sr.DeserializeUserConnections()); userConnections = conns.ToArray (); sr.DeserializeNodes(); //Assign correct graph indices. Issue #21 for (int i=0;i<graphs.Length;i++) { if (graphs[i] == null) continue; graphs[i].GetNodes (delegate (GraphNode node) { //GraphNode[] nodes = graphs[i].nodes; node.GraphIndex = (uint)i; return true; }); } sr.DeserializeExtraInfo(); sr.PostDeserialization(); for (int i=0;i<graphs.Length;i++) { for (int j=i+1;j<graphs.Length;j++) { if (graphs[i] != null && graphs[j] != null && graphs[i].guid == graphs[j].guid) { Debug.LogWarning ("Guid Conflict when importing graphs additively. Imported graph will get a new Guid.\nThis message is (relatively) harmless."); graphs[i].guid = Pathfinding.Util.Guid.NewGuid (); break; } } } }
public static void SetTagField (GUIContent label, ref Pathfinding.TagMask value) { GUILayout.BeginHorizontal (); EditorGUIUtility.LookLikeControls(); EditorGUILayout.PrefixLabel (label,EditorStyles.layerMaskField); string text = ""; if (value.tagsChange == 0) text = "Nothing"; else if (value.tagsChange == ~0) text = "Everything"; else { text = System.Convert.ToString (value.tagsChange,2); } string[] tagNames = AstarPath.FindTagNames (); if (GUILayout.Button (text,EditorStyles.layerMaskField,GUILayout.ExpandWidth (true))) { GenericMenu menu = new GenericMenu (); menu.AddItem (new GUIContent ("Everything"),value.tagsChange == ~0, value.SetValues, new Pathfinding.TagMask (~0,value.tagsSet)); menu.AddItem (new GUIContent ("Nothing"),value.tagsChange == 0, value.SetValues, new Pathfinding.TagMask (0,value.tagsSet)); for (int i=0;i<tagNames.Length;i++) { bool on = (value.tagsChange >> i & 0x1) != 0; Pathfinding.TagMask result = new Pathfinding.TagMask (on ? value.tagsChange & ~(1 << i) : value.tagsChange | 1<<i,value.tagsSet); menu.AddItem (new GUIContent (tagNames[i]),on,value.SetValues, result); } menu.AddItem (new GUIContent ("Edit Tags..."),false,AstarPathEditor.EditTags); menu.ShowAsContext (); Event.current.Use (); } #if UNITY_LE_4_3 EditorGUIUtility.LookLikeInspector(); #endif GUILayout.EndHorizontal (); }
public static void TagsMaskField (GUIContent changeLabel, GUIContent setLabel,ref Pathfinding.TagMask value) { GUILayout.BeginHorizontal (); EditorGUIUtility.LookLikeControls(); EditorGUILayout.PrefixLabel (changeLabel,EditorStyles.layerMaskField); string text = ""; if (value.tagsChange == 0) text = "Nothing"; else if (value.tagsChange == ~0) text = "Everything"; else { text = System.Convert.ToString (value.tagsChange,2); } if (GUILayout.Button (text,EditorStyles.layerMaskField,GUILayout.ExpandWidth (true))) { GenericMenu menu = new GenericMenu (); menu.AddItem (new GUIContent ("Everything"),value.tagsChange == ~0, value.SetValues, new Pathfinding.TagMask (~0,value.tagsSet)); menu.AddItem (new GUIContent ("Nothing"),value.tagsChange == 0, value.SetValues, new Pathfinding.TagMask (0,value.tagsSet)); for (int i=0;i<32;i++) { bool on = (value.tagsChange >> i & 0x1) != 0; Pathfinding.TagMask result = new Pathfinding.TagMask (on ? value.tagsChange & ~(1 << i) : value.tagsChange | 1<<i,value.tagsSet); menu.AddItem (new GUIContent (""+i),on,value.SetValues, result); } menu.ShowAsContext (); Event.current.Use (); } #if UNITY_LE_4_3 EditorGUIUtility.LookLikeInspector(); #endif GUILayout.EndHorizontal (); GUILayout.BeginHorizontal (); EditorGUIUtility.LookLikeControls(); EditorGUILayout.PrefixLabel (setLabel,EditorStyles.layerMaskField); text = ""; if (value.tagsSet == 0) text = "Nothing"; else if (value.tagsSet == ~0) text = "Everything"; else { text = System.Convert.ToString (value.tagsSet,2); } if (GUILayout.Button (text,EditorStyles.layerMaskField,GUILayout.ExpandWidth (true))) { GenericMenu menu = new GenericMenu (); if (value.tagsChange != 0) menu.AddItem (new GUIContent ("Everything"),value.tagsSet == ~0, value.SetValues, new Pathfinding.TagMask (value.tagsChange,~0)); else menu.AddDisabledItem (new GUIContent ("Everything")); menu.AddItem (new GUIContent ("Nothing"),value.tagsSet == 0, value.SetValues, new Pathfinding.TagMask (value.tagsChange,0)); for (int i=0;i<32;i++) { bool enabled = (value.tagsChange >> i & 0x1) != 0; bool on = (value.tagsSet >> i & 0x1) != 0; Pathfinding.TagMask result = new Pathfinding.TagMask (value.tagsChange, on ? value.tagsSet & ~(1 << i) : value.tagsSet | 1<<i); if (enabled) menu.AddItem (new GUIContent (""+i),on,value.SetValues, result); else menu.AddDisabledItem (new GUIContent (""+i)); } menu.ShowAsContext (); Event.current.Use (); } #if UNITY_LE_4_3 EditorGUIUtility.LookLikeInspector(); #endif GUILayout.EndHorizontal (); }
protected override void Initialize() { this.keyboard = new KeyboardManager(); this.grid = new Grid(ROW, COLUMN); matrizNodes = new Node[ROW, COLUMN]; General.ROW = ROW; General.COLUMN = COLUMN; this.lRaffle = new List<Node>(); this.lPathfinding = new List<Node>(); this.pathfinding = new General(); base.Initialize(); }