private AgentController() { if (InstanceManagers.Count > byte.MaxValue) { throw new System.Exception("Cannot have more than 256 AgentControllers"); } OpenLocalIDs.FastClear(); PeakLocalID = 0; ControllerID = (byte)InstanceManagers.Count; for (int i = 0; i < InstanceManagers.Count; i++) { this.SetAllegiance(InstanceManagers [i], AllegianceType.Neutral); } UpdateDiplomacy(this); InstanceManagers.Add(this); this.SetAllegiance(this, AllegianceType.Friendly); }
public void RaycastMove(Vector3d delta) { #if true Vector3d nextPosition = this.Position; nextPosition.Add(ref delta); HitBodies.FastClear(); foreach (LSBody body in Raycaster.RaycastAll(this.Position, nextPosition)) { if (this.BodyConditional(body)) { HitBodies.Add(body); } } if (HitBodies.Count > 0) { Hit(); } this.Position = nextPosition; #endif }
void SaveBodies() { LSBody[] allBodies = GameObject.FindObjectsOfType <LSBody> (); FastList <EnvironmentBodyInfo> bodiesBuffer = new FastList <EnvironmentBodyInfo>(); foreach (LSBody body in allBodies) { if (IsAgent(body)) { continue; } EnvironmentBodyInfo bodyInfo = new EnvironmentBodyInfo( body, new Vector2dHeight(body.transform.position), new Vector2d(Mathf.Sin(body.transform.rotation.x), Mathf.Cos(body.transform.rotation.y)) ); bodiesBuffer.Add(bodyInfo); } _environmentBodies = bodiesBuffer.ToArray(); }
void SaveObjects() { EnvironmentObject[] allObjects = GameObject.FindObjectsOfType <EnvironmentObject>(); FastList <EnvironmentObject> objectBuffer = new FastList <EnvironmentObject>(); foreach (EnvironmentObject obj in allObjects) { if (IsAgent(obj)) { continue; } objectBuffer.Add(obj); } _environmentObjects = objectBuffer.ToArray(); for (int i = 0; i < _environmentObjects.Length; i++) { _environmentObjects[i].Save(); #if UNITY_EDITOR UnityEditor.EditorUtility.SetDirty(_environmentObjects[i]); #endif } }
public void Add <TData>(TData item) where TData : ICommandData { ushort dataID; if (RegisteredData.TryGetValue(typeof(TData), out dataID)) { FastList <ICommandData> items = GetItemsList(dataID); if (items.Count == ushort.MaxValue) { throw new Exception("No more than '{0}' of a type can be added to a Command."); } if (items.Count == 0) { ContainedTypesCount++; } items.Add(item); } else { throw new System.Exception(string.Format("Type '{0}' not registered.", typeof(TData))); } }
private static void DestinationReached() { outputPath.FastClear(); TracePath.FastClear(); currentNode = endNode; StartNodeIndex = startNode.gridIndex; while (currentNode.gridIndex != StartNodeIndex) { TracePath.Add(currentNode); oldNode = currentNode; currentNode = currentNode.parent; } #if true oldX = 0; oldY = 0; currentNode = TracePath[TracePath.Count - 1]; for (i = TracePath.Count - 2; i >= 0; i--) { oldNode = currentNode; currentNode = TracePath.innerArray [i]; newX = currentNode.gridX - oldNode.gridX; newY = currentNode.gridY - oldNode.gridY; #if true if (newX != oldX || newY != oldY) { outputPath.Add(oldNode); oldX = newX; oldY = newY; } #else outputPath.Add(currentNode); #endif } #endif }
/// <summary> /// Registers an object and returns a ticket to access variable info about the object. /// Note: Ticket may vary on multiple clients and sessions. /// </summary> /// <param name="lockstepObject">Lockstep object.</param> public static int Register(object lockstepObject) { Type type = lockstepObject.GetType(); string[] propertyNames; LSVariableContainer container; if (!CachedLockstepPropertyNames.TryGetValue(type, out propertyNames)) { bufferPropertyNames.FastClear(); container = new LSVariableContainer(GetVariables(lockstepObject, type)); foreach (LSVariable info in container.Variables) { bufferPropertyNames.Add(info.Info.Name); } CachedLockstepPropertyNames.Add(type, bufferPropertyNames.ToArray()); } else { container = new LSVariableContainer(GetVariables(lockstepObject, type, propertyNames)); } return(Containers.Add(container)); }
public void GetCoveredSnappedPositions(long snapSpacing, FastList <Vector2d> output) { //long referenceX = 0, //referenceY = 0; long xmin = GetFlooredSnap(this.XMin - FixedMath.Half, snapSpacing); long ymin = GetFlooredSnap(this.YMin - FixedMath.Half, snapSpacing); long xmax = GetCeiledSnap(this.XMax + FixedMath.Half - xmin, snapSpacing) + xmin; long ymax = GetCeiledSnap(this.YMax + FixedMath.Half - ymin, snapSpacing) + ymin; //Used for getting snapped positions this body covered for (long x = xmin; x < xmax; x += snapSpacing) { for (long y = ymin; y < ymax; y += snapSpacing) { Vector2d checkPos = new Vector2d(x, y); if (IsPositionCovered(checkPos)) { output.Add(checkPos); } } } }
void SaveBodies() { UnityLSBody[] allBodies = GameObject.FindObjectsOfType <UnityLSBody>(); FastList <EnvironmentBodyInfo> bodiesBuffer = new FastList <EnvironmentBodyInfo>(); foreach (UnityLSBody body in allBodies) { if (IsAgent(body)) { continue; } Vector3d pos = new Vector3d(body.transform.position); Vector2d rot = Vector2d.CreateRotation(body.transform.eulerAngles.y * Mathf.Deg2Rad); EnvironmentBodyInfo bodyInfo = new EnvironmentBodyInfo( body, pos, rot ); bodiesBuffer.Add(bodyInfo); } _environmentBodies = bodiesBuffer.ToArray(); }
/// <summary> /// Reconstructs this command from a serialized command and returns the size of the command. /// </summary> public int Reconstruct(byte[] Source, int StartIndex = 0) { reader.Initialize(Source, StartIndex); ControllerID = reader.ReadByte(); LeInput = reader.ReadUShort(); this.ContainedTypesCount = reader.ReadUShort(); for (int i = 0; i < this.ContainedTypesCount; i++) { ushort dataID = reader.ReadUShort(); ushort dataCount = reader.ReadUShort(); FastList <ICommandData> items = GetItemsList(dataID); Type dataType = RegisteredData.GetReversed(dataID); for (int j = 0; j < dataCount; j++) { ICommandData item = Activator.CreateInstance(dataType) as ICommandData; item.Read(reader); items.Add(item); } } return(reader.Position - StartIndex); }
public void GetCoveredNodePositions(long resolution, FastList <Vector2d> output) { long xmin = GetFlooredSnap(this.XMin - FixedMath.Half, FixedMath.One); long ymin = GetFlooredSnap(this.YMin - FixedMath.Half, FixedMath.One); long xmax = GetCeiledSnap(this.XMax + FixedMath.Half - xmin, FixedMath.One) + xmin; long ymax = GetCeiledSnap(this.YMax + FixedMath.Half - ymin, FixedMath.One) + ymin; long xAcc = 0; long yAcc = 0; for (long x = xmin; x < xmax;) { for (long y = ymin; y < ymax;) { Vector2d checkPos = new Vector2d(x + xAcc, y + xAcc); if (IsPositionCovered(checkPos)) { output.Add(checkPos); } yAcc += resolution; if (yAcc >= FixedMath.One) { //Move on to next node position yAcc -= FixedMath.One; y += FixedMath.One; } } xAcc += resolution; if (xAcc >= FixedMath.One) { xAcc -= FixedMath.One; x += FixedMath.One; } } }
//Adds the agent and returns a ticket number public void Add(LSInfluencer influencer) { byte controllerID = influencer.Agent.Controller.ControllerID; if (AgentBuckets.Count <= controllerID) { //fill up indices up till the desired bucket's index for (int i = controllerID - AgentBuckets.Count; i >= 0; i--) { AgentBuckets.Add(null); } } FastBucket<LSInfluencer> bucket = AgentBuckets[controllerID]; if (bucket == null) { //A new bucket for the controller must be created bucket = new FastBucket<LSInfluencer>(); AgentBuckets[controllerID] = bucket; } influencer.NodeTicket = bucket.Add(influencer); AgentCount++; }
public static void SendCommand(Command com) { OutCommands.Add(com); }
public static void DestroyAgent(LSAgent agent, bool immediate = false) { DeactivationBuffer.Add(new DeactivationData(agent, immediate)); }
/// <summary> /// Finds a path and outputs it to <c>OutputPath</c>. Note: OutputPath is unpredictably changed. /// </summary> /// <returns> /// Returns <c>true</c> if path was found and necessary, <c>false</c> if path to End is impossible or not found. /// </returns> /// <param name="startNode">Start node.</param> /// <param name="endNode">End node.</param> /// <param name="OutputPath">Return path.</param> public static bool FindPath(GridNode startNode, GridNode endNode, FastList <GridNode> OutputPath) { #region Broadphase and Preperation if (endNode.Unwalkable) { return(false); } if (startNode.Unwalkable) { return(false); } if (true) { #region Obstruction Test //Tests if there is a direct path. If there is, no need to run AStar. x0 = startNode.gridX; y0 = startNode.gridY; x1 = endNode.gridX; y1 = endNode.gridY; if (y1 > y0) { compare1 = y1 - y0; } else { compare1 = y0 - y1; } if (x1 > x0) { compare2 = x1 - x0; } else { compare2 = x0 - x1; } steep = compare1 > compare2; if (steep) { t = x0; // swap x0 and y0 x0 = y0; y0 = t; t = x1; // swap x1 and y1 x1 = y1; y1 = t; } if (x0 > x1) { t = x0; // swap x0 and x1 x0 = x1; x1 = t; t = y0; // swap y0 and y1 y0 = y1; y1 = t; } dx = x1 - x0; dy = (y1 - y0); if (dy < 0) { dy = -dy; } error = dx / 2; ystep = (y0 < y1) ? 1 : -1; y = y0; for (x = x0; x <= x1; x++) { retX = (steep ? y : x); retY = (steep ? x : y); if (GridManager.Grid [retX * GridManager.NodeCount + retY].Unwalkable) { break; } else if (x == x1) { OutputPath.FastClear(); OutputPath.Add(startNode); OutputPath.Add(endNode); return(true); } error = error - dy; if (error < 0) { y += ystep; error += dx; } } #endregion } GridHeap.FastClear(); GridClosedSet.FastClear(); #endregion #region AStar Algorithm GridHeap.Add(startNode); GridNode.HeuristicTargetX = endNode.gridX; GridNode.HeuristicTargetY = endNode.gridY; while (GridHeap.Count > 0) { currentNode = GridHeap.RemoveFirst(); GridClosedSet.Add(currentNode); if (currentNode.gridIndex == endNode.gridIndex) { OutputPath.FastClear(); //Retraces the path then outputs it into OutputPath //Also Simplifies the path oldNode = endNode; currentNode = endNode.parent; oldX = int.MaxValue; oldY = int.MaxValue; StartNodeIndex = startNode.gridIndex; //if (!endNode.Obstructed) OutputPath.Add (endNode); while (oldNode.gridIndex != StartNodeIndex) { newX = currentNode.gridX - oldNode.gridX; newY = currentNode.gridY - oldNode.gridY; if ((newX != oldX || newY != oldY)) { OutputPath.Add(oldNode); oldX = newX; oldY = newY; } oldNode = currentNode; currentNode = currentNode.parent; } OutputPath.Add(startNode); OutputPath.Reverse(); return(true); } for (i = 0; i < 8; i++) { neighbor = currentNode.NeighborNodes [i]; if (neighbor == null || neighbor.Unwalkable || GridClosedSet.Contains(neighbor)) { continue; } newMovementCostToNeighbor = currentNode.gCost + (currentNode.NeighborDiagnal [i] ? 141 : 100); if (!GridHeap.Contains(neighbor)) { neighbor.gCost = newMovementCostToNeighbor; //Optimized heuristic calculation neighbor.CalculateHeurustic(); neighbor.parent = currentNode; GridHeap.Add(neighbor); } else if (newMovementCostToNeighbor < neighbor.gCost) { neighbor.gCost = newMovementCostToNeighbor; //Optimized heuristic calculation neighbor.CalculateHeurustic(); neighbor.parent = currentNode; GridHeap.UpdateItem(neighbor); } } } #endregion return(false); }
public void AddImmovable(int item) { ContainedImmovableObjects.Add(item); }
/// <summary> /// Finds a path and outputs it to <c>outputPath</c>. Note: outputPath is unpredictably changed. /// </summary> /// <returns> /// Returns <c>true</c> if path was found and necessary, <c>false</c> if path to End is impossible or not found. /// </returns> /// <param name="startNode">Start node.</param> /// <param name="endNode">End node.</param> /// <param name="outputPath">Return path.</param> public static bool FindPath(GridNode _startNode, GridNode _endNode, FastList <GridNode> _outputPath, int _unitSize = 1) { startNode = _startNode; endNode = _endNode; outputPath = _outputPath; unitSize = _unitSize; #region Broadphase and Preperation if (endNode.Unwalkable) { return(false); } if (startNode.Unwalkable) { return(false); } outputPath.FastClear(); if (System.Object.ReferenceEquals(startNode, endNode)) { outputPath.Add(endNode); return(true); } GridHeap.FastClear(); GridClosedSet.FastClear(); #endregion #region AStar Algorithm GridHeap.Add(startNode); GridNode.HeuristicTargetX = endNode.gridX; GridNode.HeuristicTargetY = endNode.gridY; GridNode.PrepareUnpassableCheck(unitSize); //Prepare Unpassable check optimizations while (GridHeap.Count > 0) { currentNode = GridHeap.RemoveFirst(); #if false Gizmos.DrawCube(currentNode.WorldPos.ToVector3(), Vector3.one); #endif GridClosedSet.Add(currentNode); if (currentNode.gridIndex == endNode.gridIndex) { //Retraces the path then outputs it into outputPath //Also Simplifies the path DestinationReached(); return(true); } /* * for (i = 0; i < 8; i++) { * neighbor = currentNode.NeighborNodes [i]; * if (CheckNeighborInvalid ()) { * //continue; * //microoptimization... continue is more expensive than letting the loop pass at the end * } else { * //0-3 = sides, 4-7 = diagonals * if (i < 4) { * newMovementCostToNeighbor = currentNode.gCost + 100; * } else { * if (i == 4) { * if (!GridManager.UseDiagonalConnections) * break; * } * newMovementCostToNeighbor = currentNode.gCost + 141; * } * * AnalyzeNode(); * } * } */ hasInvalidEdge = false; for (int i = 0; i < 4; i++) { neighbor = currentNode.NeighborNodes[i]; if (CheckNeighborInvalid()) { hasInvalidEdge = true; } else { newMovementCostToNeighbor = currentNode.gCost + 100; AnalyzeNode(); } } if (hasInvalidEdge) { const int maxCornerObstructions = 1; #region inlining diagonals neighbor = currentNode.NeighborNodes[4]; if (!CheckNeighborInvalid()) { if (GetObstructionCount(0, 1) <= maxCornerObstructions) { newMovementCostToNeighbor = currentNode.gCost + 141; AnalyzeNode(); } } neighbor = currentNode.NeighborNodes[5]; if (!CheckNeighborInvalid()) { if (GetObstructionCount(0, 2) <= maxCornerObstructions) { newMovementCostToNeighbor = currentNode.gCost + 141; AnalyzeNode(); } } neighbor = currentNode.NeighborNodes[6]; if (!CheckNeighborInvalid()) { if (GetObstructionCount(3, 1) <= maxCornerObstructions) { newMovementCostToNeighbor = currentNode.gCost + 141; AnalyzeNode(); } } neighbor = currentNode.NeighborNodes[7]; if (!CheckNeighborInvalid()) { if (GetObstructionCount(3, 2) <= maxCornerObstructions) { newMovementCostToNeighbor = currentNode.gCost + 141; AnalyzeNode(); } } #endregion } else { //no need for specific stuff when edges are all valid for (int i = 4; i < 8; i++) { neighbor = currentNode.NeighborNodes[i]; if (CheckNeighborInvalid()) { } else { newMovementCostToNeighbor = currentNode.gCost + 141; AnalyzeNode(); } } } } #endregion return(false); }
public bool Overlaps(FastList <Vector2d> outputIntersectionPoints) { outputIntersectionPoints.FastClear(); //Checks if this object overlaps the line formed by p1 and p2 switch (this.Shape) { case ColliderType.Circle: { bool overlaps = false; //Check if the circle completely fits between the line long projPos = this._position.Dot(cacheAxis.x, cacheAxis.y); //Circle withing bounds? if (projPos >= axisMin && projPos <= axisMax) { long projPerp = this._position.Dot(cacheAxisNormal.x, cacheAxisNormal.y); long perpDif = (cacheProjPerp - projPerp); long perpDist = perpDif.Abs(); if (perpDist <= _radius) { overlaps = true; } if (overlaps) { long sin = (perpDif); long cos = FixedMath.Sqrt(_radius.Mul(_radius) - sin.Mul(sin)); if (cos == 0) { outputIntersectionPoints.Add((cacheAxis * projPos) + perpVector); } else { outputIntersectionPoints.Add(cacheAxis * (projPos - cos) + perpVector); outputIntersectionPoints.Add(cacheAxis * (projPos + cos) + perpVector); } } } else { //If not, check distances to points long p1Dist = _position.FastDistance(cacheP1.x, cacheP2.y); if (p1Dist <= this.FastRadius) { outputIntersectionPoints.Add(cacheP1); overlaps = true; } long p2Dist = _position.FastDistance(cacheP2.x, cacheP2.y); if (p2Dist <= this.FastRadius) { outputIntersectionPoints.Add(cacheP2); overlaps = true; } } return(overlaps); } break; case ColliderType.AABox: { } break; case ColliderType.Polygon: { bool intersected = false; for (int i = 0; i < this.Vertices.Length; i++) { int edgeIndex = i; Vector2d pivot = this.RealPoints [edgeIndex]; Vector2d edge = this.Edges [edgeIndex]; long proj1 = 0; int nextIndex = edgeIndex + 1 < this.RealPoints.Length ? edgeIndex + 1 : 0; Vector2d nextPoint = RealPoints [nextIndex]; long proj2 = (nextPoint - pivot).Dot(edge); long min; long max; if (proj1 < proj2) { min = proj1; max = proj2; } else { min = proj2; max = proj1; } long lineProj1 = (cacheP1 - pivot).Dot(edge); long lineProj2 = (cacheP2 - pivot).Dot(edge); long lineMin; long lineMax; if (lineProj1 < lineProj2) { lineMin = lineProj1; lineMax = lineProj2; } else { lineMin = lineProj2; lineMax = lineProj1; } if (CollisionPair.CheckOverlap(min, max, lineMin, lineMax)) { Vector2d edgeNorm = this.EdgeNorms [edgeIndex]; long normProj = 0; long normLineProj1 = (cacheP1 - pivot).Dot(edgeNorm); long normLineProj2 = (cacheP2 - pivot).Dot(edgeNorm); long normLineMin; long normLineMax; if (normLineProj1 < normLineProj2) { normLineMin = normLineProj1; normLineMax = normLineProj2; } else { normLineMin = normLineProj2; normLineMax = normLineProj1; } if (normProj >= normLineMin && normProj <= normLineMax) { long revProj1 = pivot.Dot(LSBody.cacheAxisNormal); long revProj2 = nextPoint.Dot(cacheAxisNormal); long revMin; long revMax; if (revProj1 < revProj2) { revMin = revProj1; revMax = revProj2; } else { revMin = revProj2; revMax = revProj1; } if (LSBody.cacheProjPerp >= revMin && LSBody.cacheProjPerp <= revMax) { intersected = true; if (LSBody.calculateIntersections) { long fraction = normLineProj1.Abs().Div(normLineMax - normLineMin); long intersectionProj = FixedMath.Lerp(lineProj1, lineProj2, fraction); outputIntersectionPoints.Add(edge * intersectionProj + pivot); if (outputIntersectionPoints.Count == 2) { break; } } } } } } return(intersected); } break; } return(false); }
public static void AddInput(InputCode inputCode, KeyCode keyCode) { inputPairs.Add(new InputPair(inputCode, keyCode)); InputCount++; }
public void Write(byte value) { canvas.Add(value); }
public void TestFlash() { flashRoutines.Add(base.StartCoroutine(_TestFlash())); }
public static void ScanAll(Vector2d position, long radius, Func <LSAgent, bool> agentConditional, Func <byte, bool> bucketConditional, FastList <LSAgent> output) { //If radius is too big and we scan too many tiles, performance will be bad const long circleCastRadius = FixedMath.One * 16; output.FastClear(); if (radius >= circleCastRadius) { bufferBodies.FastClear(); PhysicsTool.CircleCast(position, radius, bufferBodies); for (int i = 0; i < bufferBodies.Count; i++) { var body = bufferBodies [i]; var agent = body.Agent; //we have to check agent's controller since we did not filter it through buckets if (bucketConditional(agent.Controller.ControllerID)) { if (agentConditional(agent)) { output.Add(agent); } } } return; } int xMin = ((position.x - radius - GridManager.OffsetX) / (long)GridManager.ScanResolution).ToInt(); int xMax = ((position.x + radius - GridManager.OffsetX) / (long)GridManager.ScanResolution).CeilToInt(); int yMin = ((position.y - radius - GridManager.OffsetY) / (long)GridManager.ScanResolution).ToInt(); int yMax = ((position.y + radius - GridManager.OffsetY) / (long)GridManager.ScanResolution).CeilToInt(); long fastRadius = radius * radius; for (int x = xMin; x <= xMax; x++) { for (int y = yMin; y <= yMax; y++) { ScanNode tempNode = GridManager.GetScanNode( x, y); if (tempNode.IsNotNull()) { if (tempNode.AgentCount > 0) { bufferBuckets.FastClear(); tempNode.GetBucketsWithAllegiance(bucketConditional, bufferBuckets); for (int i = 0; i < bufferBuckets.Count; i++) { FastBucket <LSInfluencer> tempBucket = bufferBuckets[i]; BitArray arrayAllocation = tempBucket.arrayAllocation; for (int j = 0; j < tempBucket.PeakCount; j++) { if (arrayAllocation.Get(j)) { LSAgent tempAgent = tempBucket[j].Agent; long distance = (tempAgent.Body.Position - position).FastMagnitude(); if (distance < fastRadius) { if (agentConditional(tempAgent)) { output.Add(tempAgent); } } else { } } } } } } } } }
public void AddAgents (params LSAgent[] agents) { for (int i = 0; i < agents.Length; i++) { selectedAgentLocalIDs.Add(agents[i].LocalID); } }
/// <summary> /// Finds a path and outputs it to <c>outputPath</c>. Note: outputPath is unpredictably changed. /// </summary> /// <returns> /// Returns <c>true</c> if path was found and necessary, <c>false</c> if path to End is impossible or not found. /// </returns> /// <param name="startNode">Start node.</param> /// <param name="endNode">End node.</param> /// <param name="outputPath">Return path.</param> public static bool FindPath(GridNode _startNode, GridNode _endNode, FastList <GridNode> _outputPath, int _unitSize = 1) { startNode = _startNode; endNode = _endNode; outputPath = _outputPath; unitSize = _unitSize; #region Broadphase and Preperation if (endNode.Unwalkable) { return(false); } if (startNode.Unwalkable) { return(false); } outputPath.FastClear(); if (System.Object.ReferenceEquals(startNode, endNode)) { outputPath.Add(endNode); return(true); } GridHeap.FastClear(); GridClosedSet.FastClear(); #endregion #region AStar Algorithm GridHeap.Add(startNode); GridNode.HeuristicTargetX = endNode.gridX; GridNode.HeuristicTargetY = endNode.gridY; GridNode.PrepareUnpassableCheck(unitSize); //Prepare Unpassable check optimizations while (GridHeap.Count > 0) { currentNode = GridHeap.RemoveFirst(); GridClosedSet.Add(currentNode); if (currentNode.gridIndex == endNode.gridIndex) { //Retraces the path then outputs it into outputPath //Also Simplifies the path DestinationReached(); return(true); } for (i = 0; i < 8; i++) { neighbor = currentNode.NeighborNodes [i]; if (neighbor.IsNull() || currentNode.Unpassable() || GridClosedSet.Contains(neighbor)) { continue; } //0-3 = sides, 4-7 = diagonals if (i < 4) { newMovementCostToNeighbor = currentNode.gCost + 100; } else { if (i == 4) { if (!GridManager.UseDiagonalConnections) { break; } } newMovementCostToNeighbor = currentNode.gCost + 141; } if (!GridHeap.Contains(neighbor)) { neighbor.gCost = newMovementCostToNeighbor; //Optimized heuristic calculation neighbor.CalculateHeuristic(); neighbor.parent = currentNode; GridHeap.Add(neighbor); } else if (newMovementCostToNeighbor < neighbor.gCost) { neighbor.gCost = newMovementCostToNeighbor; //Optimized heuristic calculation neighbor.CalculateHeuristic(); neighbor.parent = currentNode; GridHeap.UpdateItem(neighbor); } } } #endregion return(false); }
public static void SmoothPath(FastList <GridNode> nodePath, Vector2d End, FastList <Vector2d> outputVectorPath, int unitSize) { outputVectorPath.FastClear(); length = nodePath.Count - 1; //culling out unneded nodes var StartNode = nodePath[0]; outputVectorPath.Add(StartNode.WorldPos); GridNode oldNode = StartNode; long oldX = 0; long oldY = 0; long newX = 0; long newY = 0; for (i = 1; i < length; i++) { GridNode node = nodePath[i]; bool important = false; if (unitSize <= 1) { important = !node.Clearance; } else if (unitSize <= 3) { important = !node.ExtraClearance; } else { important = true; } //important = true; if (important) { newX = node.gridX - oldNode.gridX; newY = node.gridY - oldNode.gridY; if ( (newX <= 1 && newX >= -1) && (newY <= 1 && newY >= -1) ) { if (newX == oldX && newY == oldY) { if (oldX != 0 || oldY != 0) { outputVectorPath.RemoveAt(outputVectorPath.Count - 1); } } else { oldX = newX; oldY = newY; } } else { oldX = 0; oldY = 0; } outputVectorPath.Add(node.WorldPos); oldNode = node; } } outputVectorPath.Add(End); }
private bool InternalRaycast(Vector2d From, Vector2d To, int ExceptionID) { _Version++; MadeContact = false; Hits.FastClear(); const int StepSize = 1 << Partition.ShiftSize; x0 = From.x; y0 = From.y; x1 = To.x; y1 = To.y; if (y1 > y0) { compare1 = y1 - y0; } else { compare1 = y0 - y1; } if (x1 > x0) { compare2 = x1 - x0; } else { compare2 = x0 - x1; } steep = compare1 > compare2; if (steep) { t = x0; // swap x0 and y0 x0 = y0; y0 = t; t = x1; // swap x1 and y1 x1 = y1; y1 = t; } if (x0 > x1) { t = x0; // swap x0 and x1 x0 = x1; x1 = t; t = y0; // swap y0 and y1 y0 = y1; y1 = t; } dx = x1 - x0; dy = (y1 - y0); if (dy < 0) { dy = -dy; } error = dx / 2; ystep = (y0 < y1) ? StepSize : -StepSize; y = y0; AxisX = From.x - To.x; AxisY = From.y - To.y; Mag = FixedMath.Sqrt((AxisX * AxisX + AxisY * AxisY) >> FixedMath.SHIFT_AMOUNT); if (Mag == 0) { return(false); } AxisX = FixedMath.Div(AxisX, Mag); AxisY = FixedMath.Div(AxisY, Mag); AxisMin = Vector2d.Dot(AxisX, AxisY, From.x, From.y); AxisMax = Vector2d.Dot(AxisX, AxisY, To.x, To.y); if (AxisMin > AxisMax) { SwapValue = AxisMin; AxisMin = AxisMax; AxisMax = SwapValue; } PerpProj = Vector2d.Dot(-AxisY, AxisX, From.x, From.y); XMin = From.x; XMax = To.x; if (XMin > XMax) { SwapValue = XMin; XMin = XMax; XMax = SwapValue; } YMin = From.y; YMax = To.y; if (YMin > YMax) { SwapValue = YMin; YMin = YMax; YMax = SwapValue; } x = x0; while (true) { if (steep) { retX = (y - Partition.OffsetX) / StepSize; retY = (x - Partition.OffsetY) / StepSize; } else { retX = (x - Partition.OffsetX) / StepSize; retY = (y - Partition.OffsetY) / StepSize; } PartitionNode node = Partition.Nodes [retX * Partition.Count + retY]; if (node.Count > 0) { for (i = 0; i < node.Count; i++) { DidHit = false; LSBody body = PhysicsManager.SimObjects [node [i]]; if (body.RaycastVersion != _Version && body.ID != ExceptionID) { body.RaycastVersion = _Version; switch (body.Shape) { case ColliderType.Circle: Projection = Vector2d.Dot(AxisX, AxisY, body.Position.x, body.Position.y); TestMin = Projection - body.Radius; TestMax = Projection + body.Radius; if (TestMin < AxisMax) { if (TestMax > AxisMin) { Projection = Vector2d.Dot(-AxisY, AxisX, body.Position.x, body.Position.y); TestMin = Projection - body.Radius; TestMax = Projection + body.Radius; if (PerpProj < TestMax && PerpProj > TestMin) { DidHit = true; } } } break; case ColliderType.AABox: if (AxisMin < body.XMax) { if (AxisMax > body.XMin) { if (PerpProj < body.YMax) { if (PerpProj > body.YMin) { DidHit = true; } } } } break; } if (DidHit) { Hits.Add(body); MadeContact = true; break; } } } } error = error - dy; if (error < 0) { y += ystep; error += dx; } if (x >= x1) { break; } x += StepSize; } return(MadeContact); }
/// <summary> /// Finds a path and outputs it to <c>outputPath</c>. Note: outputPath is unpredictably changed. /// </summary> /// <returns> /// Returns <c>true</c> if path was found and necessary, <c>false</c> if path to End is impossible or not found. /// </returns> /// <param name="startNode">Start node.</param> /// <param name="endNode">End node.</param> /// <param name="outputPath">Return path.</param> public static bool FindPath(GridNode startNode, GridNode endNode, FastList <GridNode> outputPath) { #region Broadphase and Preperation if (endNode.Unwalkable) { return(false); } if (startNode.Unwalkable) { return(false); } outputPath.FastClear(); if (System.Object.ReferenceEquals(startNode, endNode)) { outputPath.Add(endNode); return(true); } System.Diagnostics.Stopwatch sw = new System.Diagnostics.Stopwatch(); sw.Start(); GridHeap.FastClear(); GridClosedSet.FastClear(); #endregion #region AStar Algorithm GridHeap.Add(startNode); GridNode.HeuristicTargetX = endNode.gridX; GridNode.HeuristicTargetY = endNode.gridY; while (GridHeap.Count > 0) { currentNode = GridHeap.RemoveFirst(); GridClosedSet.Add(currentNode); if (currentNode.gridIndex == endNode.gridIndex) { //Retraces the path then outputs it into outputPath //Also Simplifies the path outputPath.FastClear(); TracePath.FastClear(); currentNode = endNode; StartNodeIndex = startNode.gridIndex; while (currentNode.gridIndex != StartNodeIndex) { TracePath.Add(currentNode); oldNode = currentNode; currentNode = currentNode.parent; } oldNode = startNode; currentNode = TracePath [TracePath.Count - 1]; oldX = currentNode.gridX - oldNode.gridX; oldY = currentNode.gridY - oldNode.gridY; for (i = TracePath.Count - 2; i >= 0; i--) { oldNode = currentNode; currentNode = TracePath.innerArray [i]; newX = currentNode.gridX - oldNode.gridX; newY = currentNode.gridY - oldNode.gridY; if (newX != oldX || newY != oldY) { outputPath.Add(oldNode); oldX = newX; oldY = newY; } //outputPath.Add (currentNode); } outputPath.Add(endNode); return(true); } for (i = 0; i < 8; i++) { neighbor = currentNode.NeighborNodes [i]; if (neighbor == null || neighbor.Unwalkable || GridClosedSet.Contains(neighbor)) { continue; } newMovementCostToNeighbor = currentNode.gCost + (GridNode.IsNeighborDiagnal [i] ? 141 : 100); if (!GridHeap.Contains(neighbor)) { neighbor.gCost = newMovementCostToNeighbor; //Optimized heuristic calculation neighbor.CalculateHeuristic(); neighbor.parent = currentNode; GridHeap.Add(neighbor); } else if (newMovementCostToNeighbor < neighbor.gCost) { neighbor.gCost = newMovementCostToNeighbor; //Optimized heuristic calculation neighbor.CalculateHeuristic(); neighbor.parent = currentNode; GridHeap.UpdateItem(neighbor); } } } #endregion return(false); }