internal void UpdateOverlaps(Grid2DSortAndSweep owner) { //Sort along x axis using insertion sort; the list will be nearly sorted, so very few swaps are necessary. for (int i = 1; i < entries.Count; i++) { var entry = entries.Elements[i]; for (int j = i - 1; j >= 0; j--) { if (entry.item.boundingBox.Min.X < entries.Elements[j].item.boundingBox.Min.X) { entries.Elements[j + 1] = entries.Elements[j]; entries.Elements[j] = entry; } else break; } } //Sweep the list looking for overlaps. for (int i = 0; i < entries.Count; i++) { Grid2DEntry a = entries.Elements[i]; Grid2DEntry b; //TODO: Microoptimize for (int j = i + 1; j < entries.Count && a.item.boundingBox.Max.X >= (b = entries.Elements[j]).item.boundingBox.Min.X; j++) { if (!(a.item.boundingBox.Min.Y > b.item.boundingBox.Max.Y || a.item.boundingBox.Max.Y < b.item.boundingBox.Min.Y || a.item.boundingBox.Min.Z > b.item.boundingBox.Max.Z || a.item.boundingBox.Max.Z < b.item.boundingBox.Min.Z)) { //Now we know this pair is overlapping, but we do not know if this overlap is already added. //Rather than use a hashset or other heavy structure to check, rely on the rules of the grid. //It's possible to avoid adding pairs entirely unless we are the designated 'responsible' cell. //All other cells will defer to the cell 'responsible' for a pair. //A simple rule for determining the cell which is responsible is to choose the cell which is the //smallest index in the shared cells. So first, compute that cell's index. Int2 minimumSharedIndex = a.previousMin; if (minimumSharedIndex.Y < b.previousMin.Y) minimumSharedIndex.Y = b.previousMin.Y; if (minimumSharedIndex.Y > b.previousMax.Y) minimumSharedIndex.Y = b.previousMax.Y; if (minimumSharedIndex.Z < b.previousMin.Z) minimumSharedIndex.Z = b.previousMin.Z; if (minimumSharedIndex.Z > b.previousMax.Z) minimumSharedIndex.Z = b.previousMax.Z; //Is our cell the minimum cell? if (minimumSharedIndex.Y == cellIndex.Y && minimumSharedIndex.Z == cellIndex.Z) owner.TryToAddOverlap(a.item, b.item); } } } }
public void GetEntries(Microsoft.Xna.Framework.BoundingSphere boundingShape, IList <BroadPhaseEntry> overlaps) { //Create a bounding box based on the bounding sphere. //Compute the min and max of the bounding box. //Loop through the cells and select bounding boxes which overlap the x axis. #if !WINDOWS Vector3 offset = new Vector3(); #else Vector3 offset; #endif offset.X = boundingShape.Radius; offset.Y = offset.X; offset.Z = offset.Y; BoundingBox box; Vector3.Add(ref boundingShape.Center, ref offset, out box.Max); Vector3.Subtract(ref boundingShape.Center, ref offset, out box.Min); Int2 min, max; Grid2DSortAndSweep.ComputeCell(ref box.Min, out min); Grid2DSortAndSweep.ComputeCell(ref box.Max, out max); for (int i = min.Y; i <= max.Y; i++) { for (int j = min.Z; j <= max.Z; j++) { //Grab the cell that we are currently in. Int2 cellIndex; cellIndex.Y = i; cellIndex.Z = j; GridCell2D cell; if (owner.cellSet.TryGetCell(ref cellIndex, out cell)) { //To fully accelerate this, the entries list would need to contain both min and max interval markers. //Since it only contains the sorted min intervals, we can't just start at a point in the middle of the list. //Consider some giant bounding box that spans the entire list. for (int k = 0; k < cell.entries.count && cell.entries.Elements[k].item.boundingBox.Min.X <= box.Max.X; k++) //TODO: Try additional x axis pruning? A bit of optimization potential due to overlap with AABB test. { bool intersects; var item = cell.entries.Elements[k].item; boundingShape.Intersects(ref item.boundingBox, out intersects); if (intersects && !overlaps.Contains(item)) { overlaps.Add(item); } } } } } }
public bool RayCast(Microsoft.Xna.Framework.Ray ray, float maximumLength, IList <BroadPhaseEntry> outputIntersections) { if (maximumLength == float.MaxValue) { throw new NotSupportedException("The Grid2DSortAndSweep broad phase cannot accelerate infinite ray casts. Consider specifying a maximum length or using a broad phase which supports infinite ray casts."); } //Use 2d line rasterization. //Compute the exit location in the cell. //Test against each bounding box up until the exit value is reached. float length = 0; Int2 cellIndex; Vector3 currentPosition = ray.Position; Grid2DSortAndSweep.ComputeCell(ref currentPosition, out cellIndex); while (true) { float cellWidth = 1 / Grid2DSortAndSweep.cellSizeInverse; float nextT; //Distance along ray to next boundary. float nextTy; //Distance along ray to next boundary along y axis. float nextTz; //Distance along ray to next boundary along z axis. //Find the next cell. if (ray.Direction.Y > 0) { nextTy = ((cellIndex.Y + 1) * cellWidth - currentPosition.Y) / ray.Direction.Y; } else if (ray.Direction.Y < 0) { nextTy = ((cellIndex.Y) * cellWidth - currentPosition.Y) / ray.Direction.Y; } else { nextTy = 10e10f; } if (ray.Direction.Z > 0) { nextTz = ((cellIndex.Z + 1) * cellWidth - currentPosition.Z) / ray.Direction.Z; } else if (ray.Direction.Z < 0) { nextTz = ((cellIndex.Z) * cellWidth - currentPosition.Z) / ray.Direction.Z; } else { nextTz = 10e10f; } bool yIsMinimum = nextTy < nextTz; nextT = yIsMinimum ? nextTy : nextTz; //Grab the cell that we are currently in. GridCell2D cell; if (owner.cellSet.TryGetCell(ref cellIndex, out cell)) { float endingX; if (ray.Direction.X < 0) { endingX = currentPosition.X; } else { endingX = currentPosition.X + ray.Direction.X * nextT; } //To fully accelerate this, the entries list would need to contain both min and max interval markers. //Since it only contains the sorted min intervals, we can't just start at a point in the middle of the list. //Consider some giant bounding box that spans the entire list. for (int i = 0; i < cell.entries.count && cell.entries.Elements[i].item.boundingBox.Min.X <= endingX; i++) //TODO: Try additional x axis pruning? { float?intersects; var item = cell.entries.Elements[i].item; ray.Intersects(ref item.boundingBox, out intersects); if (intersects != null && intersects < maximumLength && !outputIntersections.Contains(item)) { outputIntersections.Add(item); } } } //Move the position forward. length += nextT; if (length > maximumLength) //Note that this catches the case in which the ray is pointing right down the middle of a row (resulting in a nextT of 10e10f). { break; } Vector3 offset; Vector3.Multiply(ref ray.Direction, nextT, out offset); Vector3.Add(ref offset, ref currentPosition, out currentPosition); if (yIsMinimum) { if (ray.Direction.Y < 0) { cellIndex.Y -= 1; } else { cellIndex.Y += 1; } } else if (ray.Direction.Z < 0) { cellIndex.Z -= 1; } else { cellIndex.Z += 1; } } return(outputIntersections.Count > 0); }
public Grid2DSortAndSweepQueryAccelerator(Grid2DSortAndSweep owner) { this.owner = owner; }
internal void Initialize(BroadPhaseEntry entry) { this.item = entry; Grid2DSortAndSweep.ComputeCell(ref entry.boundingBox.Min, out previousMin); Grid2DSortAndSweep.ComputeCell(ref entry.boundingBox.Max, out previousMax); }
public Grid2DSortAndSweepQueryAccelerator(Grid2DSortAndSweep owner) { this.owner = owner; }
internal void UpdateOverlaps(Grid2DSortAndSweep owner) { //Sort along x axis using insertion sort; the list will be nearly sorted, so very few swaps are necessary. for (int i = 1; i < entries.count; i++) { var entry = entries.Elements[i]; for (int j = i - 1; j >= 0; j--) { if (entry.item.boundingBox.Min.X < entries.Elements[j].item.boundingBox.Min.X) { entries.Elements[j + 1] = entries.Elements[j]; entries.Elements[j] = entry; } else { break; } } } //Sweep the list looking for overlaps. for (int i = 0; i < entries.count; i++) { Grid2DEntry a = entries.Elements[i]; Grid2DEntry b; //TODO: Microoptimize for (int j = i + 1; j < entries.count && a.item.boundingBox.Max.X >= (b = entries.Elements[j]).item.boundingBox.Min.X; j++) { if (!(a.item.boundingBox.Min.Y > b.item.boundingBox.Max.Y || a.item.boundingBox.Max.Y < b.item.boundingBox.Min.Y || a.item.boundingBox.Min.Z > b.item.boundingBox.Max.Z || a.item.boundingBox.Max.Z < b.item.boundingBox.Min.Z)) { //Now we know this pair is overlapping, but we do not know if this overlap is already added. //Rather than use a hashset or other heavy structure to check, rely on the rules of the grid. //It's possible to avoid adding pairs entirely unless we are the designated 'responsible' cell. //All other cells will defer to the cell 'responsible' for a pair. //A simple rule for determining the cell which is responsible is to choose the cell which is the //smallest index in the shared cells. So first, compute that cell's index. Int2 minimumSharedIndex = a.previousMin; if (minimumSharedIndex.Y < b.previousMin.Y) { minimumSharedIndex.Y = b.previousMin.Y; } if (minimumSharedIndex.Y > b.previousMax.Y) { minimumSharedIndex.Y = b.previousMax.Y; } if (minimumSharedIndex.Z < b.previousMin.Z) { minimumSharedIndex.Z = b.previousMin.Z; } if (minimumSharedIndex.Z > b.previousMax.Z) { minimumSharedIndex.Z = b.previousMax.Z; } //Is our cell the minimum cell? if (minimumSharedIndex.Y == cellIndex.Y && minimumSharedIndex.Z == cellIndex.Z) { owner.TryToAddOverlap(a.item, b.item); } } } } }
/// <summary> /// Constructs a new demo. /// </summary> /// <param name="game">Game owning this demo.</param> public BroadPhasesTestDemo(DemosGame game) : base(game) { Space.Solver.IterationLimit = 0; Entity toAdd; //BoundingBox box = new BoundingBox(new Vector3(-5, 1, 1), new Vector3(5, 7, 7)); BoundingBox box = new BoundingBox(new Vector3(-50, -50, -50), new Vector3(50, 50, 50)); //DynamicHierarchyOld dhOld = new DynamicHierarchyOld(Space.ThreadManager); DynamicHierarchy dh = new DynamicHierarchy(Space.ParallelLooper); SortAndSweep1D sas1d = new SortAndSweep1D(Space.ParallelLooper); Grid2DSortAndSweep grid2DSAS = new Grid2DSortAndSweep(Space.ParallelLooper); //DynamicHierarchy dh = new DynamicHierarchy(); //DynamicHierarchy4 dh4 = new DynamicHierarchy4(); //SortAndSweep1D sas1d = new SortAndSweep1D(); //Grid2DSortAndSweep grid2DSAS = new Grid2DSortAndSweep(); //DynamicHierarchy2 dh2 = new DynamicHierarchy2(); //DynamicHierarchy3 dh3 = new DynamicHierarchy3(); //SortAndSweep3D sap3d = new SortAndSweep3D(); RawList<Entity> entities = new RawList<Entity>(); for (int k = 0; k < 100; k++) { Vector3 position = new Vector3((float)(rand.NextDouble() * (box.Max.X - box.Min.X) + box.Min.X), (float)(rand.NextDouble() * (box.Max.Y - box.Min.Y) + box.Min.Y), (float)(rand.NextDouble() * (box.Max.Z - box.Min.Z) + box.Min.Z)); toAdd = new Box(position, 1, 1, 1, 1); toAdd.CollisionInformation.CollisionRules.Personal = CollisionRule.NoNarrowPhasePair; toAdd.CollisionInformation.UpdateBoundingBox(0); //Space.Add(toAdd); //dhOld.Add(toAdd.CollisionInformation); dh.Add(toAdd.CollisionInformation); sas1d.Add(toAdd.CollisionInformation); grid2DSAS.Add(toAdd.CollisionInformation); entities.Add(toAdd); } Space.ForceUpdater.Gravity = new Vector3(); int numRuns = 10000; //Prime the system. grid2DSAS.Update(); sas1d.Update(); //dhOld.Update(); dh.Update(); var testType = Test.Update; double startTime, endTime; switch (testType) { #region Update Timing case Test.Update: for (int i = 0; i < numRuns; i++) { ////DH //startTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency; //dhOld.Update(); //endTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency; //DHOldTime += endTime - startTime; //DH4 startTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency; dh.Update(); endTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency; DHtime += endTime - startTime; //SAP1D startTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency; sas1d.Update(); endTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency; SAS1Dtime += endTime - startTime; //Grid2D SOS startTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency; grid2DSAS.Update(); endTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency; grid2DSAStime += endTime - startTime; //if (sap1d.Overlaps.Count != dh.Overlaps.Count) // Debug.WriteLine("SAP1D Failure"); //if (grid2DSOS.Overlaps.Count != dh.Overlaps.Count) // Debug.WriteLine("grid2DSOS Failure"); //for (int j = 0; j < dh2.Overlaps.Count; j++) //{ // if (!grid2DSOS.Overlaps.Contains(dh2.Overlaps[j])) // Debug.WriteLine("Break."); //} //for (int j = 0; j < grid2DSOS.Overlaps.Count; j++) //{ // if (!dh2.Overlaps.Contains(grid2DSOS.Overlaps[j])) // break; //} //for (int j = 0; j < grid2DSOS.Overlaps.Count; j++) //{ // if (!dh4.Overlaps.Contains(grid2DSOS.Overlaps[j])) // break; //} //for (int j = 0; j < dh.Overlaps.Count; j++) //{ // if (!dh.Overlaps[j].EntryA.BoundingBox.Intersects(dh.Overlaps[j].EntryB.BoundingBox)) // Debug.WriteLine("Break."); //} //for (int j = 0; j < sap1d.Overlaps.Count; j++) //{ // if (!sap1d.Overlaps[j].EntryA.BoundingBox.Intersects(sap1d.Overlaps[j].EntryB.BoundingBox)) // Debug.WriteLine("Break."); //} //for (int j = 0; j < grid2DSOS.Overlaps.Count; j++) //{ // if (!grid2DSOS.Overlaps[j].EntryA.BoundingBox.Intersects(grid2DSOS.Overlaps[j].EntryB.BoundingBox)) // Debug.WriteLine("Break."); //} //MoveEntities(entities); } break; #endregion #region Ray cast timing case Test.RayCast: float rayLength = 100; RawList<Ray> rays = new RawList<Ray>(); for (int i = 0; i < numRuns; i++) { rays.Add(new Ray() { Position = new Vector3((float)(rand.NextDouble() * (box.Max.X - box.Min.X) + box.Min.X), (float)(rand.NextDouble() * (box.Max.Y - box.Min.Y) + box.Min.Y), (float)(rand.NextDouble() * (box.Max.Z - box.Min.Z) + box.Min.Z)), Direction = Vector3.Normalize(new Vector3((float)(rand.NextDouble() - .5), (float)(rand.NextDouble() - .5), (float)(rand.NextDouble() - .5))) }); } RawList<BroadPhaseEntry> outputIntersections = new RawList<BroadPhaseEntry>(); ////DH //startTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency; //for (int i = 0; i < numRuns; i++) //{ // dhOld.QueryAccelerator.RayCast(rays.Elements[i], rayLength, outputIntersections); // outputIntersections.Clear(); //} //endTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency; //DHOldTime = endTime - startTime; //DH4 startTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency; for (int i = 0; i < numRuns; i++) { dh.QueryAccelerator.RayCast(rays.Elements[i], rayLength, outputIntersections); outputIntersections.Clear(); } endTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency; DHtime = endTime - startTime; //Grid2DSAS startTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency; for (int i = 0; i < numRuns; i++) { grid2DSAS.QueryAccelerator.RayCast(rays.Elements[i], rayLength, outputIntersections); outputIntersections.Clear(); } endTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency; grid2DSAStime = endTime - startTime; break; #endregion #region Bounding box query timing case Test.BoundingBoxQuery: float boundingBoxSize = 10; var boundingBoxes = new RawList<BoundingBox>(); Vector3 offset = new Vector3(boundingBoxSize / 2, boundingBoxSize / 2, boundingBoxSize / 2); for (int i = 0; i < numRuns; i++) { Vector3 center = new Vector3((float)(rand.NextDouble() * (box.Max.X - box.Min.X) + box.Min.X), (float)(rand.NextDouble() * (box.Max.Y - box.Min.Y) + box.Min.Y), (float)(rand.NextDouble() * (box.Max.Z - box.Min.Z) + box.Min.Z)); boundingBoxes.Add(new BoundingBox() { Min = center - offset, Max = center + offset }); } outputIntersections = new RawList<BroadPhaseEntry>(); ////DH //startTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency; //for (int i = 0; i < numRuns; i++) //{ // dhOld.QueryAccelerator.GetEntries(boundingBoxes.Elements[i], outputIntersections); // outputIntersections.Clear(); //} //endTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency; //DHOldTime = endTime - startTime; //DH4 startTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency; for (int i = 0; i < numRuns; i++) { dh.QueryAccelerator.GetEntries(boundingBoxes.Elements[i], outputIntersections); outputIntersections.Clear(); } endTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency; DHtime = endTime - startTime; //Grid2DSAS startTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency; for (int i = 0; i < numRuns; i++) { grid2DSAS.QueryAccelerator.GetEntries(boundingBoxes.Elements[i], outputIntersections); outputIntersections.Clear(); } endTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency; grid2DSAStime = endTime - startTime; break; #endregion } DHOldTime /= numRuns; DH2time /= numRuns; DH3time /= numRuns; DHtime /= numRuns; SAS1Dtime /= numRuns; grid2DSAStime /= numRuns; }