///<summary>
/// Cleans up the pair handler.
///</summary>
public override void CleanUp()
{
//Fire off some events if needed! Note the order; we should stop containing before we stop touching.
if (Parent == null)
{
if (Containing)
{
DetectorVolume.StoppedContaining(this);
}
if (Touching)
{
DetectorVolume.StoppedTouching(this);
}
}
Containing = false;
Touching = false;
WasContaining = false;
WasTouching = false;
DetectorVolume.pairs.Remove(Collidable.entity);
broadPhaseOverlap = new BroadPhaseOverlap();
DetectorVolume = null;
Parent = null;
//Child cleanup is responsible for cleaning up direct references to the involved collidables.
}
void UpdateBroadPhaseOverlap(int i)
{
BroadPhaseOverlap overlap = broadPhaseOverlaps.Elements[i];
if (overlap.collisionRule < CollisionRule.NoNarrowPhasePair)
{
NarrowPhasePair pair;
//see if the overlap is already present in the narrow phase.
if (!overlapMapping.TryGetValue(overlap, out pair))
{
//Create/enqueue based on collision table
pair = NarrowPhaseHelper.GetPairHandler(ref overlap);
if (pair != null)
{
pair.NarrowPhase = this;
//Add the new object to the 'todo' list.
//Technically, this doesn't need to be thread-safe when this is called from the sequential context.
//It's just bunched together for maintainability despite the slight performance hit.
newNarrowPhasePairs.Enqueue(pair);
}
}
if (pair != null)
{
//Update the collision rule.
pair.CollisionRule = overlap.collisionRule;
if (pair.BroadPhaseOverlap.collisionRule < CollisionRule.NoNarrowPhaseUpdate)
{
pair.UpdateCollision(TimeStepSettings.TimeStepDuration);
}
pair.NeedsUpdate = false;
}
}
}
///<summary>
/// Cleans up the pair handler.
///</summary>
public override void CleanUp()
{
//Child types remove contacts from the pair handler and call OnContactRemoved.
//Child types manage the removal of the constraint from the space, if necessary.
//If the contact manifold had any contacts in it on cleanup, then we still need to fire the 'ending' event.
if (previousContactCount > 0 && !suppressEvents)
{
CollidableA.EventTriggerer.OnCollisionEnded(CollidableB, this);
CollidableB.EventTriggerer.OnCollisionEnded(CollidableA, this);
}
//Remove this pair from each collidable. This can be done safely because the CleanUp is called sequentially.
//However, only do it if we have been added to the collidables! This does not happen until this pair is added to the narrow phase.
//For pairs which never get added to the broad phase, such as those in queries, we should not attempt to remove something that isn't there!
if (listIndexA != -1)
{
CollidableA.RemovePair(this, ref listIndexA);
CollidableB.RemovePair(this, ref listIndexB);
}
//Notify the colliders that the pair went away.
if (!suppressEvents)
{
CollidableA.EventTriggerer.OnPairRemoved(CollidableB);
CollidableB.EventTriggerer.OnPairRemoved(CollidableA);
}
broadPhaseOverlap = new BroadPhaseOverlap();
suppressEvents = false;
timeOfImpact = 1;
Parent = null;
previousContactCount = 0;
//Child cleanup is responsible for cleaning up direct references to the involved collidables.
//Child cleanup is responsible for cleaning up contact manifolds.
}
public override void Initialize(BroadPhaseEntry entryA, BroadPhaseEntry entryB)
{
if (noRecurse)
{
return;
}
noRecurse = true;
mesh = entryA as MobileChunkCollidable;
mobile = entryB as MobileChunkCollidable;
if (mesh == null || mobile == null)
{
mesh = entryB as MobileChunkCollidable;
mobile = entryA as MobileChunkCollidable;
if (mesh == null || mobile == null)
{
throw new ArgumentException("Inappropriate types used to initialize pair.");
}
}
broadPhaseOverlap = new BroadPhaseOverlap(mobile, mesh, broadPhaseOverlap.CollisionRule);
UpdateMaterialProperties(mobile.Entity?.Material, mesh.Entity?.Material);
base.Initialize(entryA, entryB);
noRecurse = false;
}
public override void Initialize(BroadPhaseEntry entryA, BroadPhaseEntry entryB)
{
if (noRecurse)
{
return;
}
noRecurse = true;
mesh = entryA as FullChunkObject;
convex = entryB as ConvexCollidable;
if (mesh == null || convex == null)
{
mesh = entryB as FullChunkObject;
convex = entryA as ConvexCollidable;
if (mesh == null || convex == null)
{
throw new ArgumentException("Inappropriate types used to initialize pair.");
}
}
broadPhaseOverlap = new BroadPhaseOverlap(convex, mesh, broadPhaseOverlap.CollisionRule);
UpdateMaterialProperties(convex.Entity != null ? convex.Entity.Material : null, mesh.Material);
base.Initialize(entryA, entryB);
noRecurse = false;
}
///<summary>
/// Cleans up the pair handler.
///</summary>
public override void CleanUp()
{
//Child types remove contacts from the pair handler and call OnContactRemoved.
//Child types manage the removal of the constraint from the space, if necessary.
//If the contact manifold had any contacts in it on cleanup, then we still need to fire the 'ending' event.
if (previousContactCount > 0 && !suppressEvents)
{
CollidableA.EventTriggerer.OnCollisionEnded(CollidableB, this);
CollidableB.EventTriggerer.OnCollisionEnded(CollidableA, this);
}
//Remove this pair from each collidable. This can be done safely because the CleanUp is called sequentially.
CollidableA.pairs.Remove(this);
CollidableB.pairs.Remove(this);
//Notify the colliders that the pair went away.
if (!suppressEvents)
{
CollidableA.EventTriggerer.OnPairRemoved(CollidableB);
CollidableB.EventTriggerer.OnPairRemoved(CollidableA);
}
broadPhaseOverlap = new BroadPhaseOverlap();
(this as NarrowPhasePair).NeedsUpdate = false;
(this as NarrowPhasePair).NarrowPhase = null;
suppressEvents = false;
timeOfImpact = 1;
Parent = null;
previousContactCount = 0;
//Child cleanup is responsible for cleaning up direct references to the involved collidables.
//Child cleanup is responsible for cleaning up contact manifolds.
}
///<summary>
/// Gets a narrow phase pair for a given broad phase overlap.
///</summary>
///<param name="pair">Overlap to use to create the pair.</param>
///<returns>A INarrowPhasePair for the overlap.</returns>
public static NarrowPhasePair GetPairHandler(ref BroadPhaseOverlap pair)
{
NarrowPhasePairFactory factory;
if (collisionManagers.TryGetValue(new TypePair(pair.entryA.GetType(), pair.entryB.GetType()), out factory))
{
var toReturn = factory.GetNarrowPhasePair();
toReturn.BroadPhaseOverlap = pair;
toReturn.Factory = factory;
return(toReturn);
}
//Convex-convex collisions are a pretty significant chunk of all tests, so rather than defining them all, just have a fallback.
var a = pair.entryA as ConvexCollidable;
var b = pair.entryB as ConvexCollidable;
if (a != null && b != null)
{
NarrowPhasePair toReturn = Factories.ConvexConvex.GetNarrowPhasePair();
toReturn.BroadPhaseOverlap = pair;
toReturn.Factory = Factories.ConvexConvex;
return(toReturn);
}
return(null);
}
public override void Initialize(BroadPhaseEntry entryA, BroadPhaseEntry entryB)
{
voxelGrid = entryA as VoxelGrid;
convex = entryB as ConvexCollidable;
if (voxelGrid == null || convex == null)
{
voxelGrid = entryB as VoxelGrid;
convex = entryA as ConvexCollidable;
if (voxelGrid == null || convex == null)
{
throw new ArgumentException("Inappropriate types used to initialize pair.");
}
}
//Contact normal goes from A to B.
broadPhaseOverlap = new BroadPhaseOverlap(convex, voxelGrid, broadPhaseOverlap.CollisionRule);
UpdateMaterialProperties(convex.Entity != null ? convex.Entity.Material : null, voxelGrid.Material);
base.Initialize(entryA, entryB);
}
double RunTest(int splitOffset, IParallelLooper parallelLooper)
{
Entity toAdd;
//BoundingBox box = new BoundingBox(new Vector3(-5, 1, 1), new Vector3(5, 7, 7));
BoundingBox box = new BoundingBox(new Vector3(-500, -500, -500), new Vector3(500, 500, 500));
int splitDepth = splitOffset + (int)Math.Ceiling(Math.Log(parallelLooper.ThreadCount, 2));
DynamicHierarchy dh = new DynamicHierarchy(parallelLooper);
Random rand = new Random(0);
RawList <Entity> entities = new RawList <Entity>();
for (int k = 0; k < 10000; k++)
{
Vector3 position = new Vector3((Fix64)rand.NextDouble() * (box.Max.X - box.Min.X) + box.Min.X,
(Fix64)rand.NextDouble() * (box.Max.Y - box.Min.Y) + box.Min.Y,
(Fix64)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);
dh.Add(toAdd.CollisionInformation);
entities.Add(toAdd);
}
Space.ForceUpdater.Gravity = new Vector3();
int numRuns = 3000;
//Prime the system.
dh.Update();
var testType = Test.Update;
BroadPhaseOverlap[] overlapBasis = new BroadPhaseOverlap[dh.Overlaps.Count];
dh.Overlaps.CopyTo(overlapBasis, 0);
double time = 0;
double startTime, endTime;
switch (testType)
{
#region Update Timing
case Test.Update:
for (int i = 0; i < numRuns; i++)
{
//DH4
startTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
//dh.Update();
//lock (dh.Locker)
//{
// dh.Overlaps.Clear();
// if (dh.ROOTEXISTS)
// {
// dh.MultithreadedRefitPhase(splitDepth);
// dh.MultithreadedOverlapPhase(splitDepth);
// }
//}
//dh.Update();
//lock (dh.Locker)
//{
// dh.Overlaps.Clear();
// if (dh.ROOTEXISTS)
// {
// dh.SingleThreadedRefitPhase();
// dh.SingleThreadedOverlapPhase();
// }
//}
endTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
time += endTime - startTime;
//if (dh.Overlaps.Count != overlapBasis.Length)
// Debug.WriteLine("Failed Update.");
//for (int j = 0; j < overlapBasis.Length; j++)
//{
// if (!dh.Overlaps.Contains(overlapBasis[j]))
// Debug.WriteLine("Failed Update.");
//}
//MoveEntities(entities);
}
break;
#endregion
#region Refit Timing
case Test.Refit:
for (int i = 0; i < numRuns; i++)
{
dh.Overlaps.Clear();
//DH4
startTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
//dh.MultithreadedRefitPhase(splitDepth);
//dh.SingleThreadedRefitPhase();
endTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
time += endTime - startTime;
//dh.SingleThreadedOverlapPhase();
//if (dh.Overlaps.Count != overlapBasis.Length)
// Debug.WriteLine("Failed Refit.");
//for (int j = 0; j < overlapBasis.Length; j++)
//{
// if (!dh.Overlaps.Contains(overlapBasis[j]))
// Debug.WriteLine("Failed Refit.");
//}
//MoveEntities(entities);
}
break;
#endregion
#region Overlap Timing
case Test.Overlap:
for (int i = 0; i < numRuns; i++)
{
dh.Overlaps.Clear();
//dh.MultithreadedRefitPhase(splitDepth);
//DH4
startTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
//dh.MultithreadedOverlapPhase(splitDepth);
//dh.SingleThreadedOverlapPhase();
endTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
time += endTime - startTime;
//if (dh.Overlaps.Count != overlapBasis.Length)
// Debug.WriteLine("Failed Overlap.");
//for (int j = 0; j < overlapBasis.Length; j++)
//{
// if (!dh.Overlaps.Contains(overlapBasis[j]))
// Debug.WriteLine("Failed Overlap.");
//}
//MoveEntities(entities);
}
break;
#endregion
#region Ray cast timing
case Test.RayCast:
Fix64 rayLength = 100;
RawList <Ray> rays = new RawList <Ray>();
for (int i = 0; i < numRuns; i++)
{
rays.Add(new Ray()
{
Position = new Vector3((Fix64)rand.NextDouble() * (box.Max.X - box.Min.X) + box.Min.X,
(Fix64)rand.NextDouble() * (box.Max.Y - box.Min.Y) + box.Min.Y,
(Fix64)rand.NextDouble() * (box.Max.Z - box.Min.Z) + box.Min.Z),
Direction = Vector3.Normalize(new Vector3((Fix64)(rand.NextDouble() - .5), (Fix64)(rand.NextDouble() - .5), (Fix64)(rand.NextDouble() - .5)))
});
}
RawList <BroadPhaseEntry> outputIntersections = new RawList <BroadPhaseEntry>();
//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;
time = endTime - startTime;
break;
#endregion
#region Bounding box query timing
case Test.BoundingBoxQuery:
Fix64 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((Fix64)rand.NextDouble() * (box.Max.X - box.Min.X) + box.Min.X,
(Fix64)rand.NextDouble() * (box.Max.Y - box.Min.Y) + box.Min.Y,
(Fix64)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>();
//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;
time = endTime - startTime;
break;
#endregion
}
return(time / numRuns);
}
///<summary>
/// Updates the time of impact for the pair.
///</summary>
///<param name="requester">Collidable requesting the update.</param>
///<param name="dt">Timestep duration.</param>
public override void UpdateTimeOfImpact(Collidable requester, float dt)
{
ConvexCollidable collidableA = CollidableA as ConvexCollidable;
ConvexCollidable collidableB = CollidableB as ConvexCollidable;
PositionUpdateMode modeA = collidableA.entity == null
? PositionUpdateMode.Discrete
: collidableA.entity.PositionUpdateMode;
PositionUpdateMode modeB = collidableB.entity == null
? PositionUpdateMode.Discrete
: collidableB.entity.PositionUpdateMode;
BroadPhaseOverlap overlap = BroadPhaseOverlap;
if (
(overlap.entryA.IsActive || overlap.entryB.IsActive) && //At least one has to be active.
(
modeA == PositionUpdateMode.Continuous && //If both are continuous, only do the process for A.
modeB == PositionUpdateMode.Continuous &&
overlap.entryA == requester ||
(modeA == PositionUpdateMode.Continuous) ^ //If only one is continuous, then we must do it.
(modeB == PositionUpdateMode.Continuous)
)
)
{
//Only perform the test if the minimum radii are small enough relative to the size of the velocity.
//Discrete objects have already had their linear motion integrated, so don't use their velocity.
Vector3 velocity;
if (modeA == PositionUpdateMode.Discrete)
//CollidableA is static for the purposes of this continuous test.
{
velocity = collidableB.entity.linearVelocity;
}
else if (modeB == PositionUpdateMode.Discrete)
//CollidableB is static for the purposes of this continuous test.
{
Vector3.Negate(ref collidableA.entity.linearVelocity, out velocity);
}
else
//Both objects are moving.
{
Vector3.Subtract(ref collidableB.entity.linearVelocity, ref collidableA.entity.linearVelocity,
out velocity);
}
Vector3.Multiply(ref velocity, dt, out velocity);
float velocitySquared = velocity.LengthSquared();
float minimumRadiusA = collidableA.Shape.MinimumRadius * MotionSettings.CoreShapeScaling;
timeOfImpact = 1;
if (minimumRadiusA * minimumRadiusA < velocitySquared)
{
//Spherecast A against B.
RayHit rayHit;
if (GJKToolbox.CCDSphereCast(new Ray(collidableA.worldTransform.Position, -velocity),
minimumRadiusA, collidableB.Shape, ref collidableB.worldTransform, timeOfImpact, out rayHit))
{
timeOfImpact = rayHit.T;
}
}
float minimumRadiusB = collidableB.Shape.MinimumRadius * MotionSettings.CoreShapeScaling;
if (minimumRadiusB * minimumRadiusB < velocitySquared)
{
//Spherecast B against A.
RayHit rayHit;
if (GJKToolbox.CCDSphereCast(new Ray(collidableB.worldTransform.Position, velocity), minimumRadiusB,
collidableA.Shape, ref collidableA.worldTransform, timeOfImpact, out rayHit))
{
timeOfImpact = rayHit.T;
}
}
//If it's intersecting, throw our hands into the air and give up.
//This is generally a perfectly acceptable thing to do, since it's either sitting
//inside another object (no ccd makes sense) or we're still in an intersecting case
//from a previous frame where CCD took place and a contact should have been created
//to deal with interpenetrating velocity. Sometimes that contact isn't sufficient,
//but it's good enough.
if (timeOfImpact == 0)
{
timeOfImpact = 1;
}
}
}
double RunTest(int splitOffset, IParallelLooper parallelLooper)
{
Entity toAdd;
//BoundingBox box = new BoundingBox(new Vector3(-5, 1, 1), new Vector3(5, 7, 7));
BoundingBox box = new BoundingBox(new Vector3(-500, -500, -500), new Vector3(500, 500, 500));
int splitDepth = splitOffset + (int)Math.Ceiling(Math.Log(parallelLooper.ThreadCount, 2));
DynamicHierarchy dh = new DynamicHierarchy(parallelLooper);
Random rand = new Random(0);
RawList<Entity> entities = new RawList<Entity>();
for (int k = 0; k < 10000; 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);
dh.Add(toAdd.CollisionInformation);
entities.Add(toAdd);
}
Space.ForceUpdater.Gravity = new Vector3();
int numRuns = 3000;
//Prime the system.
dh.Update();
var testType = Test.Update;
BroadPhaseOverlap[] overlapBasis = new BroadPhaseOverlap[dh.Overlaps.Count];
dh.Overlaps.CopyTo(overlapBasis, 0);
double time = 0;
double startTime, endTime;
switch (testType)
{
#region Update Timing
case Test.Update:
for (int i = 0; i < numRuns; i++)
{
//DH4
startTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
//dh.Update();
//lock (dh.Locker)
//{
// dh.Overlaps.Clear();
// if (dh.ROOTEXISTS)
// {
// dh.MultithreadedRefitPhase(splitDepth);
// dh.MultithreadedOverlapPhase(splitDepth);
// }
//}
//dh.Update();
//lock (dh.Locker)
//{
// dh.Overlaps.Clear();
// if (dh.ROOTEXISTS)
// {
// dh.SingleThreadedRefitPhase();
// dh.SingleThreadedOverlapPhase();
// }
//}
endTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
time += endTime - startTime;
//if (dh.Overlaps.Count != overlapBasis.Length)
// Debug.WriteLine("Failed Update.");
//for (int j = 0; j < overlapBasis.Length; j++)
//{
// if (!dh.Overlaps.Contains(overlapBasis[j]))
// Debug.WriteLine("Failed Update.");
//}
//MoveEntities(entities);
}
break;
#endregion
#region Refit Timing
case Test.Refit:
for (int i = 0; i < numRuns; i++)
{
dh.Overlaps.Clear();
//DH4
startTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
//dh.MultithreadedRefitPhase(splitDepth);
//dh.SingleThreadedRefitPhase();
endTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
time += endTime - startTime;
//dh.SingleThreadedOverlapPhase();
//if (dh.Overlaps.Count != overlapBasis.Length)
// Debug.WriteLine("Failed Refit.");
//for (int j = 0; j < overlapBasis.Length; j++)
//{
// if (!dh.Overlaps.Contains(overlapBasis[j]))
// Debug.WriteLine("Failed Refit.");
//}
//MoveEntities(entities);
}
break;
#endregion
#region Overlap Timing
case Test.Overlap:
for (int i = 0; i < numRuns; i++)
{
dh.Overlaps.Clear();
//dh.MultithreadedRefitPhase(splitDepth);
//DH4
startTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
//dh.MultithreadedOverlapPhase(splitDepth);
//dh.SingleThreadedOverlapPhase();
endTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;
time += endTime - startTime;
//if (dh.Overlaps.Count != overlapBasis.Length)
// Debug.WriteLine("Failed Overlap.");
//for (int j = 0; j < overlapBasis.Length; j++)
//{
// if (!dh.Overlaps.Contains(overlapBasis[j]))
// Debug.WriteLine("Failed Overlap.");
//}
//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>();
//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;
time = 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>();
//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;
time = endTime - startTime;
break;
#endregion
}
return time / numRuns;
}
///<summary>
/// Gets a narrow phase pair for a given pair of entries.
///</summary>
///<param name="entryA">First entry in the pair.</param>
/// <param name="entryB">Second entry in the pair.</param>
///<returns>AINarrowPhasePair for the overlap.</returns>
public static NarrowPhasePair GetPairHandler(BroadPhaseEntry entryA, BroadPhaseEntry entryB)
{
var overlap = new BroadPhaseOverlap(entryA, entryB);
return GetPairHandler(ref overlap);
}
/// <summary>
/// Gets a collidable pair handler for a pair of collidables.
/// </summary>
/// <param name="pair">Pair of collidables to use to create the pair handler.</param>
/// <returns>CollidablePairHandler for the pair.</returns>
public static CollidablePairHandler GetPairHandler(ref CollidablePair pair)
{
var overlap = new BroadPhaseOverlap(pair.collidableA, pair.collidableB);
return(GetPairHandler(ref overlap) as CollidablePairHandler);
}
///<summary>
/// Cleans up the pair handler.
///</summary>
public override void CleanUp()
{
//Fire off some events if needed! Note the order; we should stop containing before we stop touching.
if (Parent == null)
{
if (Containing)
{
DetectorVolume.StoppedContaining(this);
}
if (Touching)
{
DetectorVolume.StoppedTouching(this);
}
}
Containing = false;
Touching = false;
WasContaining = false;
WasTouching = false;
DetectorVolume.pairs.Remove(Collidable.entity);
broadPhaseOverlap = new BroadPhaseOverlap();
DetectorVolume = null;
Parent = null;
//Child cleanup is responsible for cleaning up direct references to the involved collidables.
}
///<summary>
/// Cleans up the pair handler.
///</summary>
public override void CleanUp()
{
//Child types remove contacts from the pair handler and call OnContactRemoved.
//Child types manage the removal of the constraint from the space, if necessary.
//If the contact manifold had any contacts in it on cleanup, then we still need to fire the 'ending' event.
if (previousContactCount > 0 && !suppressEvents)
{
CollidableA.EventTriggerer.OnCollisionEnded(CollidableB, this);
CollidableB.EventTriggerer.OnCollisionEnded(CollidableA, this);
}
//Remove this pair from each collidable. This can be done safely because the CleanUp is called sequentially.
CollidableA.pairs.Remove(this);
CollidableB.pairs.Remove(this);
//Notify the colliders that the pair went away.
if (!suppressEvents)
{
CollidableA.EventTriggerer.OnPairRemoved(CollidableB);
CollidableB.EventTriggerer.OnPairRemoved(CollidableA);
}
broadPhaseOverlap = new BroadPhaseOverlap();
(this as NarrowPhasePair).NeedsUpdate = false;
(this as NarrowPhasePair).NarrowPhase = null;
suppressEvents = false;
timeOfImpact = 1;
Parent = null;
previousContactCount = 0;
//Child cleanup is responsible for cleaning up direct references to the involved collidables.
//Child cleanup is responsible for cleaning up contact manifolds.
}
public override void Initialize(BroadPhaseEntry entryA, BroadPhaseEntry entryB)
{
if (noRecurse)
{
return;
}
noRecurse = true;
mesh = entryA as FullChunkObject;
convex = entryB as ConvexCollidable;
if (mesh == null || convex == null)
{
mesh = entryB as FullChunkObject;
convex = entryA as ConvexCollidable;
if (mesh == null || convex == null)
{
throw new ArgumentException("Inappropriate types used to initialize pair.");
}
}
broadPhaseOverlap = new BroadPhaseOverlap(convex, mesh, broadPhaseOverlap.CollisionRule);
UpdateMaterialProperties(convex.Entity != null ? convex.Entity.Material : null, mesh.Material);
base.Initialize(entryA, entryB);
noRecurse = false;
}
/// <summary>
/// Gets a collidable pair handler for a pair of collidables.
/// </summary>
/// <param name="pair">Pair of collidables to use to create the pair handler.</param>
/// <param name="rule">Collision rule governing the pair.</param>
/// <returns>CollidablePairHandler for the pair.</returns>
public static CollidablePairHandler GetPairHandler(ref CollidablePair pair, CollisionRule rule)
{
BroadPhaseOverlap overlap = new BroadPhaseOverlap(pair.collidableA, pair.collidableB, rule);
return(GetPair(ref overlap) as CollidablePairHandler);
}
///<summary>
/// Gets a narrow phase pair for a given pair of entries.
///</summary>
///<param name="entryA">First entry in the pair.</param>
/// <param name="entryB">Second entry in the pair.</param>
/// <param name="rule">Collision rule governing the pair.</param>
///<returns>A INarrowPhasePair for the overlap.</returns>
public static NarrowPhasePair GetPair(BroadPhaseEntry entryA, BroadPhaseEntry entryB, CollisionRule rule)
{
BroadPhaseOverlap overlap = new BroadPhaseOverlap(entryA, entryB, rule);
return(GetPair(ref overlap));
}
protected internal void AddOverlap(BroadPhaseOverlap overlap)
{
overlapAddLock.Enter();
overlaps.Add(overlap);
overlapAddLock.Exit();
}
///<summary>
/// Updates the time of impact for the pair.
///</summary>
///<param name="requester">Collidable requesting the update.</param>
///<param name="dt">Timestep duration.</param>
public override void UpdateTimeOfImpact(Collidable requester, float dt)
{
BroadPhaseOverlap overlap = BroadPhaseOverlap;
PositionUpdateMode meshMode = mobileMesh.entity == null
? PositionUpdateMode.Discrete
: mobileMesh.entity.PositionUpdateMode;
PositionUpdateMode convexMode =
convex.entity == null ? PositionUpdateMode.Discrete : convex.entity.PositionUpdateMode;
if (
(mobileMesh.IsActive || convex.IsActive) && //At least one has to be active.
(
convexMode == PositionUpdateMode.Continuous && //If both are continuous, only do the process for A.
meshMode == PositionUpdateMode.Continuous &&
overlap.entryA == requester ||
(convexMode == PositionUpdateMode.Continuous) ^ //If only one is continuous, then we must do it.
(meshMode == PositionUpdateMode.Continuous)
)
)
{
//TODO: This system could be made more robust by using a similar region-based rejection of edges.
//CCD events are awfully rare under normal circumstances, so this isn't usually an issue.
//Only perform the test if the minimum radii are small enough relative to the size of the velocity.
Vector3 velocity;
if (convexMode == PositionUpdateMode.Discrete)
//Convex is static for the purposes of CCD.
{
Vector3.Negate(ref mobileMesh.entity.linearVelocity, out velocity);
}
else if (meshMode == PositionUpdateMode.Discrete)
//Mesh is static for the purposes of CCD.
{
velocity = convex.entity.linearVelocity;
}
else
//Both objects can move.
{
Vector3.Subtract(ref convex.entity.linearVelocity, ref mobileMesh.entity.linearVelocity,
out velocity);
}
Vector3.Multiply(ref velocity, dt, out velocity);
float velocitySquared = velocity.LengthSquared();
float minimumRadius = convex.Shape.MinimumRadius * MotionSettings.CoreShapeScaling;
timeOfImpact = 1;
if (minimumRadius * minimumRadius < velocitySquared)
{
TriangleSidedness sidedness = mobileMesh.Shape.Sidedness;
Matrix3x3 orientation;
Matrix3x3.CreateFromQuaternion(ref mobileMesh.worldTransform.Orientation, out orientation);
TriangleShape triangle = PhysicsThreadResources.GetTriangle();
triangle.collisionMargin = 0;
//Spherecast against all triangles to find the earliest time.
for (int i = 0; i < MeshManifold.overlappedTriangles.Count; i++)
{
MeshBoundingBoxTreeData data = mobileMesh.Shape.TriangleMesh.Data;
int triangleIndex = MeshManifold.overlappedTriangles.Elements[i];
data.GetTriangle(triangleIndex, out triangle.vA, out triangle.vB, out triangle.vC);
Matrix3x3.Transform(ref triangle.vA, ref orientation, out triangle.vA);
Matrix3x3.Transform(ref triangle.vB, ref orientation, out triangle.vB);
Matrix3x3.Transform(ref triangle.vC, ref orientation, out triangle.vC);
Vector3.Add(ref triangle.vA, ref mobileMesh.worldTransform.Position, out triangle.vA);
Vector3.Add(ref triangle.vB, ref mobileMesh.worldTransform.Position, out triangle.vB);
Vector3.Add(ref triangle.vC, ref mobileMesh.worldTransform.Position, out triangle.vC);
//Put the triangle into 'localish' space of the convex.
Vector3.Subtract(ref triangle.vA, ref convex.worldTransform.Position, out triangle.vA);
Vector3.Subtract(ref triangle.vB, ref convex.worldTransform.Position, out triangle.vB);
Vector3.Subtract(ref triangle.vC, ref convex.worldTransform.Position, out triangle.vC);
RayHit rayHit;
if (GJKToolbox.CCDSphereCast(new Ray(Toolbox.ZeroVector, velocity), minimumRadius, triangle,
ref Toolbox.RigidIdentity, timeOfImpact, out rayHit) &&
rayHit.T > Toolbox.BigEpsilon)
{
if (sidedness != TriangleSidedness.DoubleSided)
{
Vector3 AB, AC;
Vector3.Subtract(ref triangle.vB, ref triangle.vA, out AB);
Vector3.Subtract(ref triangle.vC, ref triangle.vA, out AC);
Vector3 normal;
Vector3.Cross(ref AB, ref AC, out normal);
float dot;
Vector3.Dot(ref normal, ref rayHit.Normal, out dot);
//Only perform sweep if the object is in danger of hitting the object.
//Triangles can be one sided, so check the impact normal against the triangle normal.
if (sidedness == TriangleSidedness.Counterclockwise && dot < 0 ||
sidedness == TriangleSidedness.Clockwise && dot > 0)
{
timeOfImpact = rayHit.T;
}
}
else
{
timeOfImpact = rayHit.T;
}
}
}
PhysicsThreadResources.GiveBack(triangle);
}
}
}
///<summary>
/// Gets a narrow phase pair for a given pair of entries.
///</summary>
///<param name="entryA">First entry in the pair.</param>
/// <param name="entryB">Second entry in the pair.</param>
///<returns>AINarrowPhasePair for the overlap.</returns>
public static NarrowPhasePair GetPairHandler(BroadPhaseEntry entryA, BroadPhaseEntry entryB)
{
var overlap = new BroadPhaseOverlap(entryA, entryB);
return(GetPairHandler(ref overlap));
}
public override void Initialize( BroadPhaseEntry entryA, BroadPhaseEntry entryB )
{
voxelBlob = entryA as VoxelBlob;
convex = entryB as ConvexCollidable;
if( voxelBlob == null || convex == null )
{
voxelBlob = entryB as VoxelBlob;
convex = entryA as ConvexCollidable;
if( voxelBlob == null || convex == null )
throw new ArgumentException( "Inappropriate types used to initialize pair." );
}
//Contact normal goes from A to B.
broadPhaseOverlap = new BroadPhaseOverlap( convex, voxelBlob, broadPhaseOverlap.CollisionRule );
Debugger.Break();
//UpdateMaterialProperties( convex.Entity != null ? convex.Entity.Material : null, voxelBlob.Material );
base.Initialize( entryA, entryB );
}
///<summary>
/// Gets a narrow phase pair for a given broad phase overlap.
///</summary>
///<param name="pair">Overlap to use to create the pair.</param>
///<returns>A INarrowPhasePair for the overlap.</returns>
public static NarrowPhasePair GetPairHandler(ref BroadPhaseOverlap pair)
{
NarrowPhasePairFactory factory;
if (collisionManagers.TryGetValue(new TypePair(pair.entryA.GetType(), pair.entryB.GetType()), out factory))
{
var toReturn = factory.GetNarrowPhasePair();
toReturn.BroadPhaseOverlap = pair;
toReturn.Factory = factory;
return toReturn;
}
//Convex-convex collisions are a pretty significant chunk of all tests, so rather than defining them all, just have a fallback.
var a = pair.entryA as ConvexCollidable;
var b = pair.entryB as ConvexCollidable;
if (a != null && b != null)
{
NarrowPhasePair toReturn = Factories.ConvexConvex.GetNarrowPhasePair();
toReturn.BroadPhaseOverlap = pair;
toReturn.Factory = Factories.ConvexConvex;
return toReturn;
}
return null;
}
///<summary>
/// Updates the time of impact for the pair.
///</summary>
///<param name="requester">Collidable requesting the update.</param>
///<param name="dt">Timestep duration.</param>
public override void UpdateTimeOfImpact(Collidable requester, float dt)
{
BroadPhaseOverlap overlap = BroadPhaseOverlap;
PositionUpdateMode triangleMode = triangle.entity == null
? PositionUpdateMode.Discrete
: triangle.entity.PositionUpdateMode;
PositionUpdateMode convexMode =
convex.entity == null ? PositionUpdateMode.Discrete : convex.entity.PositionUpdateMode;
if (
(overlap.entryA.IsActive || overlap.entryB.IsActive) && //At least one has to be active.
(
convexMode == PositionUpdateMode.Continuous && //If both are continuous, only do the process for A.
triangleMode == PositionUpdateMode.Continuous &&
overlap.entryA == requester ||
(convexMode == PositionUpdateMode.Continuous) ^ //If only one is continuous, then we must do it.
(triangleMode == PositionUpdateMode.Continuous)
)
)
{
//Only perform the test if the minimum radii are small enough relative to the size of the velocity.
Vector3 velocity;
if (convexMode == PositionUpdateMode.Discrete)
//Triangle is static for the purposes of this continuous test.
{
velocity = triangle.entity.linearVelocity;
}
else if (triangleMode == PositionUpdateMode.Discrete)
//Convex is static for the purposes of this continuous test.
{
Vector3.Negate(ref convex.entity.linearVelocity, out velocity);
}
else
//Both objects are moving.
{
Vector3.Subtract(ref triangle.entity.linearVelocity, ref convex.entity.linearVelocity,
out velocity);
}
Vector3.Multiply(ref velocity, dt, out velocity);
float velocitySquared = velocity.LengthSquared();
float minimumRadiusA = convex.Shape.MinimumRadius * MotionSettings.CoreShapeScaling;
timeOfImpact = 1;
if (minimumRadiusA * minimumRadiusA < velocitySquared)
{
//Spherecast A against B.
RayHit rayHit;
if (GJKToolbox.CCDSphereCast(new Ray(convex.worldTransform.Position, -velocity), minimumRadiusA,
triangle.Shape, ref triangle.worldTransform, timeOfImpact, out rayHit))
{
if (triangle.Shape.sidedness != TriangleSidedness.DoubleSided)
{
//Only perform sweep if the object is in danger of hitting the object.
//Triangles can be one sided, so check the impact normal against the triangle normal.
Vector3 AB, AC;
Vector3.Subtract(ref triangle.Shape.vB, ref triangle.Shape.vA, out AB);
Vector3.Subtract(ref triangle.Shape.vC, ref triangle.Shape.vA, out AC);
Vector3 normal;
Vector3.Cross(ref AB, ref AC, out normal);
float dot;
Vector3.Dot(ref rayHit.Normal, ref normal, out dot);
if (triangle.Shape.sidedness == TriangleSidedness.Counterclockwise && dot < 0 ||
triangle.Shape.sidedness == TriangleSidedness.Clockwise && dot > 0)
{
timeOfImpact = rayHit.T;
}
}
else
{
timeOfImpact = rayHit.T;
}
}
}
//TECHNICALLY, the triangle should be casted too. But, given the way triangles are usually used and their tiny minimum radius, ignoring it is usually just fine.
//var minimumRadiusB = triangle.minimumRadius * MotionSettings.CoreShapeScaling;
//if (minimumRadiusB * minimumRadiusB < velocitySquared)
//{
// //Spherecast B against A.
// RayHit rayHit;
// if (GJKToolbox.SphereCast(new Ray(triangle.entity.position, velocity), minimumRadiusB, convex.Shape, ref convex.worldTransform, 1, out rayHit) &&
// rayHit.T < timeOfImpact)
// {
// if (triangle.Shape.sidedness != TriangleSidedness.DoubleSided)
// {
// float dot;
// Vector3.Dot(ref rayHit.Normal, ref normal, out dot);
// if (dot > 0)
// {
// timeOfImpact = rayHit.T;
// }
// }
// else
// {
// timeOfImpact = rayHit.T;
// }
// }
//}
//If it's intersecting, throw our hands into the air and give up.
//This is generally a perfectly acceptable thing to do, since it's either sitting
//inside another object (no ccd makes sense) or we're still in an intersecting case
//from a previous frame where CCD took place and a contact should have been created
//to deal with interpenetrating velocity. Sometimes that contact isn't sufficient,
//but it's good enough.
if (timeOfImpact == 0)
{
timeOfImpact = 1;
}
}
}
/// <summary>
/// Gets a collidable pair handler for a pair of collidables.
/// </summary>
/// <param name="pair">Pair of collidables to use to create the pair handler.</param>
/// <returns>CollidablePairHandler for the pair.</returns>
public static CollidablePairHandler GetPairHandler(ref CollidablePair pair)
{
var overlap = new BroadPhaseOverlap(pair.collidableA, pair.collidableB);
return GetPairHandler(ref overlap) as CollidablePairHandler;
}
/// <summary>
/// Gets a collidable pair handler for a pair of collidables.
/// </summary>
/// <param name="pair">Pair of collidables to use to create the pair handler.</param>
/// <param name="rule">Collision rule governing the pair.</param>
/// <returns>CollidablePairHandler for the pair.</returns>
public static CollidablePairHandler GetPairHandler(ref CollidablePair pair, CollisionRule rule)
{
BroadPhaseOverlap overlap = new BroadPhaseOverlap(pair.collidableA, pair.collidableB, rule);
return GetPair(ref overlap) as CollidablePairHandler;
}
protected override void UpdateSingleThreaded()
{
overlapCandidatesX.Clear();
overlapCandidatesY.Clear();
Overlaps.Clear();
//Sort along x axis using insertion sort; the list will be nearly sorted, so very few swaps are necessary.
for (int i = 1; i < entriesX.Count; i++)
{
var entry = entriesX.Elements[i];
for (int j = i - 1; j >= 0; j--)
{
if (entry.boundingBox.Min.X < entriesX.Elements[j].boundingBox.Min.X)
{
entriesX.Elements[j + 1] = entriesX.Elements[j];
entriesX.Elements[j] = entry;
}
else
break;
}
}
//Sort along y axis using insertion sort; the list will be nearly sorted, so very few swaps are necessary.
for (int i = 1; i < entriesY.Count; i++)
{
var entry = entriesY.Elements[i];
for (int j = i - 1; j >= 0; j--)
{
if (entry.boundingBox.Min.Y < entriesY.Elements[j].boundingBox.Min.Y)
{
entriesY.Elements[j + 1] = entriesY.Elements[j];
entriesY.Elements[j] = entry;
}
else
break;
}
}
//Sort along z axis using insertion sort; the list will be nearly sorted, so very few swaps are necessary.
for (int i = 1; i < entriesZ.Count; i++)
{
var entry = entriesZ.Elements[i];
for (int j = i - 1; j >= 0; j--)
{
if (entry.boundingBox.Min.Z < entriesZ.Elements[j].boundingBox.Min.Z)
{
entriesZ.Elements[j + 1] = entriesZ.Elements[j];
entriesZ.Elements[j] = entry;
}
else
break;
}
}
//Hash-set based sweeping is way too slow. 3D sap is really best suited to an incremental approach.
//Sweep the list looking for overlaps.
//Sweep the X axis first; in this phase, add overlaps to the hash set if they exist.
for (int i = 0; i < entriesX.Count; i++)
{
BoundingBox a = entriesX.Elements[i].boundingBox;
for (int j = i + 1; j < entriesX.Count && a.Max.X > entriesX.Elements[j].boundingBox.Min.X; j++)
{
overlapCandidatesX.Add(new BroadPhaseOverlap(entriesX.Elements[i], entriesX.Elements[j]));
}
}
//Sweep the Y axis second; same thing
for (int i = 0; i < entriesY.Count; i++)
{
BoundingBox a = entriesY.Elements[i].boundingBox;
for (int j = i + 1; j < entriesY.Count && a.Max.Y > entriesY.Elements[j].boundingBox.Min.Y; j++)
{
overlapCandidatesY.Add(new BroadPhaseOverlap(entriesY.Elements[i], entriesY.Elements[j]));
}
}
//Sweep the Z axis last
for (int i = 0; i < entriesZ.Count; i++)
{
BoundingBox a = entriesZ.Elements[i].boundingBox;
for (int j = i + 1; j < entriesZ.Count && a.Max.Z > entriesZ.Elements[j].boundingBox.Min.Z; j++)
{
var overlap = new BroadPhaseOverlap(entriesZ.Elements[i], entriesZ.Elements[j]);
if (overlapCandidatesX.Contains(overlap) && overlapCandidatesY.Contains(overlap))
TryToAddOverlap(entriesZ.Elements[i], entriesZ.Elements[j]);
}
}
}
///<summary>
/// Cleans up the pair handler.
///</summary>
public override void CleanUp()
{
//Child types remove contacts from the pair handler and call OnContactRemoved.
//Child types manage the removal of the constraint from the space, if necessary.
//If the contact manifold had any contacts in it on cleanup, then we still need to fire the 'ending' event.
if (previousContactCount > 0 && !suppressEvents)
{
CollidableA.EventTriggerer.OnCollisionEnded(CollidableB, this);
CollidableB.EventTriggerer.OnCollisionEnded(CollidableA, this);
}
//Remove this pair from each collidable. This can be done safely because the CleanUp is called sequentially.
//However, only do it if we have been added to the collidables! This does not happen until this pair is added to the narrow phase.
//For pairs which never get added to the broad phase, such as those in queries, we should not attempt to remove something that isn't there!
if (listIndexA != -1)
{
CollidableA.RemovePair(this, ref listIndexA);
CollidableB.RemovePair(this, ref listIndexB);
}
//Notify the colliders that the pair went away.
if (!suppressEvents)
{
CollidableA.EventTriggerer.OnPairRemoved(CollidableB);
CollidableB.EventTriggerer.OnPairRemoved(CollidableA);
}
broadPhaseOverlap = new BroadPhaseOverlap();
suppressEvents = false;
timeOfImpact = 1;
Parent = null;
previousContactCount = 0;
//Child cleanup is responsible for cleaning up direct references to the involved collidables.
//Child cleanup is responsible for cleaning up contact manifolds.
}
protected override void UpdateSingleThreaded()
{
overlapCandidatesX.Clear();
overlapCandidatesY.Clear();
Overlaps.Clear();
//Sort along x axis using insertion sort; the list will be nearly sorted, so very few swaps are necessary.
for (int i = 1; i < entriesX.count; i++)
{
var entry = entriesX.Elements[i];
for (int j = i - 1; j >= 0; j--)
{
if (entry.boundingBox.Min.X < entriesX.Elements[j].boundingBox.Min.X)
{
entriesX.Elements[j + 1] = entriesX.Elements[j];
entriesX.Elements[j] = entry;
}
else
{
break;
}
}
}
//Sort along y axis using insertion sort; the list will be nearly sorted, so very few swaps are necessary.
for (int i = 1; i < entriesY.count; i++)
{
var entry = entriesY.Elements[i];
for (int j = i - 1; j >= 0; j--)
{
if (entry.boundingBox.Min.Y < entriesY.Elements[j].boundingBox.Min.Y)
{
entriesY.Elements[j + 1] = entriesY.Elements[j];
entriesY.Elements[j] = entry;
}
else
{
break;
}
}
}
//Sort along z axis using insertion sort; the list will be nearly sorted, so very few swaps are necessary.
for (int i = 1; i < entriesZ.count; i++)
{
var entry = entriesZ.Elements[i];
for (int j = i - 1; j >= 0; j--)
{
if (entry.boundingBox.Min.Z < entriesZ.Elements[j].boundingBox.Min.Z)
{
entriesZ.Elements[j + 1] = entriesZ.Elements[j];
entriesZ.Elements[j] = entry;
}
else
{
break;
}
}
}
//Hash-set based sweeping is way too slow. 3D sap is really best suited to an incremental approach.
//Sweep the list looking for overlaps.
//Sweep the X axis first; in this phase, add overlaps to the hash set if they exist.
for (int i = 0; i < entriesX.count; i++)
{
BoundingBox a = entriesX.Elements[i].boundingBox;
for (int j = i + 1; j < entriesX.count && a.Max.X > entriesX.Elements[j].boundingBox.Min.X; j++)
{
overlapCandidatesX.Add(new BroadPhaseOverlap(entriesX.Elements[i], entriesX.Elements[j]));
}
}
//Sweep the Y axis second; same thing
for (int i = 0; i < entriesY.count; i++)
{
BoundingBox a = entriesY.Elements[i].boundingBox;
for (int j = i + 1; j < entriesY.count && a.Max.Y > entriesY.Elements[j].boundingBox.Min.Y; j++)
{
overlapCandidatesY.Add(new BroadPhaseOverlap(entriesY.Elements[i], entriesY.Elements[j]));
}
}
//Sweep the Z axis last
for (int i = 0; i < entriesZ.count; i++)
{
BoundingBox a = entriesZ.Elements[i].boundingBox;
for (int j = i + 1; j < entriesZ.count && a.Max.Z > entriesZ.Elements[j].boundingBox.Min.Z; j++)
{
var overlap = new BroadPhaseOverlap(entriesZ.Elements[i], entriesZ.Elements[j]);
if (overlapCandidatesX.Contains(overlap) && overlapCandidatesY.Contains(overlap))
{
TryToAddOverlap(entriesZ.Elements[i], entriesZ.Elements[j]);
}
}
}
}
///<summary>
/// Gets a narrow phase pair for a given pair of entries.
///</summary>
///<param name="entryA">First entry in the pair.</param>
/// <param name="entryB">Second entry in the pair.</param>
/// <param name="rule">Collision rule governing the pair.</param>
///<returns>A INarrowPhasePair for the overlap.</returns>
public static NarrowPhasePair GetPair(BroadPhaseEntry entryA, BroadPhaseEntry entryB, CollisionRule rule)
{
BroadPhaseOverlap overlap = new BroadPhaseOverlap(entryA, entryB, rule);
return GetPair(ref overlap);
}
