public CollisionPair(Geom g1, Geom g2)
{
Debug.Assert(g1 != g2);
if (g1 < g2)
{
geom1 = g1;
geom2 = g2;
}
else
{
geom1 = g2;
geom2 = g1;
}
}
public void AddPair(Geom g1, Geom g2)
{
CollisionPair cp = new CollisionPair(g1, g2);
// This check is a trade-off. In many cases, we don't need to perform
// this check and we could just do a "try" block instead, however,
// when exceptions are thrown, they are mega-slow... so checking for
// the key is really the best option all round.
if (ContainsKey(cp))
{
return;
}
Add(cp, true);
}
private void DetectInternal(bool doX)
{
List <Stub> stubs = (doX) ? (_xStubs) : (_yStubs);
LinkedList <Wrapper> currentBodies = new LinkedList <Wrapper>();
for (int index = 0; index < stubs.Count; index++)
{
Stub stub = stubs[index];
Wrapper wrapper1 = stub.wrapper;
if (stub.begin)
{
//set the min and max values
if (doX)
{
wrapper1.SetY();
}
else
{
wrapper1.SetX();
}
Geom geom1 = wrapper1.geom;
for (LinkedListNode <Wrapper> node = currentBodies.First;
node != null;
node = node.Next)
{
Wrapper wrapper2 = node.Value;
Geom geom2 = wrapper2.geom;
if (wrapper1.min <= wrapper2.max && //tests the other axis
wrapper2.min <= wrapper1.max)
{
OnCollision(geom1, geom2);
}
}
if (wrapper1.shouldAddNode)
{
currentBodies.AddLast(wrapper1.node);
}
}
else
{
if (wrapper1.shouldAddNode)
{
currentBodies.Remove(wrapper1.node);
}
}
}
}
/// <summary>
/// Computes the grid.
/// </summary>
/// <param name="geometry">The geometry.</param>
/// <param name="data"><see cref="ColliderData"/> that contains the size of the grid cell.</param>
/// <exception cref="ArgumentNullException"><c>geometry</c> is null.</exception>
public void Prepare(Geom geometry, ColliderData data)
{
float gridCellSize = data.GridCellSize;
if (geometry == null)
{
throw new ArgumentNullException("geometry", "Geometry can't be null");
}
if (gridCellSize <= 0)
{
throw new ArgumentNullException("data", "The grid cell size must be larger than 0");
}
//Prepare the geometry.
Matrix old = geometry.Matrix;
//Note: Changed in 2.1
//Matrix identity = Matrix.Identity;
//geometry.Matrix = identity;
//to:
geometry.Matrix = Matrix.Identity;
//Copy the AABB to the grid field
_aabb = new AABB(geometry.AABB);
_gridCellSize = gridCellSize;
_gridCellSizeInv = 1 / gridCellSize;
//Note: Physics2d have +2
int xSize = (int)Math.Ceiling((double)(_aabb.Max.X - _aabb.Min.X) * _gridCellSizeInv) + 1;
int ySize = (int)Math.Ceiling((double)(_aabb.Max.Y - _aabb.Min.Y) * _gridCellSizeInv) + 1;
_nodes = new float[xSize, ySize];
Vector2 vector = _aabb.Min;
for (int x = 0; x < xSize; ++x, vector.X += gridCellSize)
{
vector.Y = _aabb.Min.Y;
for (int y = 0; y < ySize; ++y, vector.Y += gridCellSize)
{
_nodes[x, y] = geometry.GetNearestDistance(vector); // shape.GetDistance(vector);
}
}
//restore the geometry
geometry.Matrix = old;
}
public void MoveUnderConsiderationToOverlaps()
{
for (int i = 0; i < Count; i++)
{
if (this[i].UnderConsideration.Count == 0)
{
continue;
}
Geom g1 = this[i].Geometry;
// First transfer those under consideration to overlaps,
// for, they have been considered...
int startIndex = this[i].Overlaps.Count;
this[i].Overlaps.AddRange(this[i].UnderConsideration);
this[i].UnderConsideration.Clear();
for (int j = startIndex; j < this[i].Overlaps.Count; j++)
{
Geom g2 = this[i].Overlaps[j];
// It is possible that we may test the same pair of geometries
// for both extents (x and y), however, I'm banking on that
// one of the extents has probably already been cached and
// therefore, won't be checked.
if (DoCollision(g1, g2) == false)
{
continue;
}
//Call the OnBroadPhaseCollision event first. If the user aborts the collision
//it will not create an arbiter
if (Owner.OnBroadPhaseCollision != null)
{
if (Owner.OnBroadPhaseCollision(g1, g2))
{
Owner.CollisionPairs.AddPair(g1, g2);
}
}
else
{
Owner.CollisionPairs.AddPair(g1, g2);
}
}
}
}
private void FillHash()
{
//Average used to optimize cell size if AutoAdjustCellSize = true.
float average = 0;
for (int i = 0; i < _physicsSimulator.GeomList.Count; i++)
{
Geom geom = _physicsSimulator.GeomList[i];
//Note: Could do some checking here for geometries that should not be included in the hashmap
AABB aabb = geom.AABB;
if (AutoAdjustCellSize)
{
average += Math.Max(aabb.Max.X - aabb.Min.X, aabb.Max.Y - aabb.Min.Y);
}
int minX = (int)(aabb.Min.X * _cellSizeInv);
int maxX = (int)(aabb.Max.X * _cellSizeInv) + 1;
int minY = (int)(aabb.Min.Y * _cellSizeInv);
int maxY = (int)(aabb.Max.Y * _cellSizeInv) + 1;
for (int x = minX; x < maxX; x++)
{
for (int y = minY; y < maxY; y++)
{
long key = PairID.GetHash(x, y);
List <Geom> list;
if (!_hash.TryGetValue(key, out list))
{
list = new List <Geom>();
_hash.Add(key, list);
}
list.Add(geom);
}
}
}
if (AutoAdjustCellSize)
{
CellSize = 2 * average / (_physicsSimulator.GeomList.Count);
}
}
private void OnCollision(Geom geom1, Geom geom2)
{
if (!geom1.body.enabled || !geom2.body.enabled)
{
return;
}
if ((geom1.collisionGroup == geom2.collisionGroup) &&
geom1.collisionGroup != 0 &&
geom2.collisionGroup != 0)
{
return;
}
if (!geom1.collisionEnabled || !geom2.collisionEnabled)
{
return;
}
if (geom1.body.isStatic && geom2.body.isStatic)
{
//don't collide two static bodies
return;
}
if (((geom1.collisionCategories & geom2.collidesWith) == CollisionCategories.None) &
((geom2.collisionCategories & geom1.collidesWith) == CollisionCategories.None))
{
return;
}
Arbiter arbiter = _physicsSimulator.arbiterPool.Fetch();
arbiter.ConstructArbiter(geom1, geom2, _physicsSimulator);
if (!_physicsSimulator.arbiterList.Contains(arbiter))
{
_physicsSimulator.arbiterList.Add(arbiter);
}
else
{
_physicsSimulator.arbiterPool.Release(arbiter);
}
}
/// <summary>
/// Detects all collision points between a line and a Geom. If intersections exist a LineIntersectionInfo
/// object is created and added to an existing list of such objects.
/// </summary>
/// <param name="p1">The first point of the line segment to test</param>
/// <param name="p2">The second point of the line segment to test</param>
/// <param name="geom">The geometry to test.</param>
/// <param name="detectUsingAABB">If true, intersection will be tested using the Geoms AABB. If false, the Geoms vertices will be used.</param>
/// <param name="lineIntersectInfoList">An existing intersect info list to add to</param>
public static void LineSegmentGeomIntersect(ref Vector2 p1, ref Vector2 p2, Geom geom, bool detectUsingAABB,
ref List <LineIntersectInfo> lineIntersectInfoList)
{
List <Vector2> points = new List <Vector2>();
if (detectUsingAABB)
{
LineSegmentAABBIntersect(ref p1, ref p2, geom.AABB, ref points);
if (points.Count > 0)
{
lineIntersectInfoList.Add(new LineIntersectInfo(geom, points));
}
}
else
{
LineSegmentVerticesIntersect(ref p1, ref p2, geom.WorldVertices, ref points);
if (points.Count > 0)
{
lineIntersectInfoList.Add(new LineIntersectInfo(geom, points));
}
}
}
/// <summary>
/// Computes the grid.
/// </summary>
/// <param name="geometry">The geometry.</param>
/// <param name="gridCellSize">Size of the grid cell.</param>
public void ComputeGrid(Geom geometry, float gridCellSize)
{
//Prepare the geometry.
Matrix old = geometry.Matrix;
//TODO: Assign geometry.Matrix to Matrix.Identity directly
Matrix identity = Matrix.Identity;
geometry.Matrix = identity;
//Copy the AABB to the grid field
_aabb = new AABB(geometry.AABB);
_gridCellSize = gridCellSize;
_gridCellSizeInv = 1 / gridCellSize;
//NOTE: Using double cast instead of converting.
int xSize = (int)Math.Ceiling((double)(_aabb.Max.X - _aabb.Min.X) * _gridCellSizeInv) + 1;
int ySize = (int)Math.Ceiling((double)(_aabb.Max.Y - _aabb.Min.Y) * _gridCellSizeInv) + 1;
//TODO: Possible optimization (normal)! If the shape is symmetric in X and Y axis, don't calculate the points, replicate them.
_nodes = new float[xSize, ySize];
_points = new Vector2[xSize * ySize];
int i = 0;
Vector2 vector = _aabb.Min;
for (int x = 0; x < xSize; ++x, vector.X += gridCellSize)
{
vector.Y = _aabb.Min.Y;
for (int y = 0; y < ySize; ++y, vector.Y += gridCellSize)
{
_nodes[x, y] = geometry.GetNearestDistance(vector); // shape.GetDistance(vector);
_points[i] = vector;
i += 1;
}
}
//restore the geometry
geometry.Matrix = old;
}
public void ComputeGrid(Geom geometry, float gridCellSize)
{
//prepare the geometry.
Matrix old = geometry.Matrix;
Matrix identity = Matrix.Identity;
geometry.Matrix = identity;
_aabb = new AABB(geometry.AABB);
_gridCellSize = gridCellSize;
_gridCellSizeInv = 1 / gridCellSize;
//TODO: Possible optimization (minor)! use casting, use _aabb.Width and Height and check if Height==Width instead of calculating twice.
int xSize = (int)Math.Ceiling(Convert.ToDouble((_aabb.Max.X - _aabb.Min.X) * _gridCellSizeInv)) + 1;
int ySize = (int)Math.Ceiling(Convert.ToDouble((_aabb.Max.Y - _aabb.Min.Y) * _gridCellSizeInv)) + 1;
_nodes = new float[xSize, ySize];
_points = new Vector2[xSize * ySize];
int i = 0;
Vector2 vector = _aabb.Min;
//TODO: Possible optimization (normal)! Cache the grids for later use. (don't recreate a grid of 64x64 if it's already made)
//TODO: Possible optimization (normal)! If the shape is symmetric in X and Y axis, don't calculate the points, replicate them.
for (int x = 0; x < xSize; ++x, vector.X += gridCellSize)
{
vector.Y = _aabb.Min.Y;
for (int y = 0; y < ySize; ++y, vector.Y += gridCellSize)
{
_nodes[x, y] = geometry.GetNearestDistance(vector); // shape.GetDistance(vector);
_points[i] = vector;
i += 1;
}
}
//restore the geometry
geometry.Matrix = old;
}
/// <summary>
/// Removes a distance grid from the cache.
/// </summary>
/// <param name="geom">The geom.</param>
public void RemoveDistanceGrid(Geom geom)
{
_distanceGrids.Remove(geom.id);
}
/// <summary>
/// Finds the contactpoints between the two geometries.
/// </summary>
/// <param name="geomA">The first geom.</param>
/// <param name="geomB">The second geom.</param>
/// <param name="contactList">The contact list.</param>
public void Collide(Geom geomA, Geom geomB, ContactList contactList)
{
int vertexIndex = -1;
//Lookup distancegrid A data from list
DistanceGridData geomAGridData = _distanceGrids[geomA.id];
//Iterate the second geometry vertices
for (int i = 0; i < geomB.worldVertices.Count; i++)
{
if (contactList.Count == PhysicsSimulator.MaxContactsToDetect)
{
break;
}
vertexIndex += 1;
_vertRef = geomB.WorldVertices[i];
geomA.TransformToLocalCoordinates(ref _vertRef, out _localVertex);
//The geometry intersects when distance <= 0
//Continue in the list if the current vector does not intersect
if (!geomAGridData.Intersect(ref _localVertex, out _feature))
{
continue;
}
//If the geometries collide, create a new contact and add it to the contact list.
if (_feature.Distance < 0f)
{
geomA.TransformNormalToWorld(ref _feature.Normal, out _feature.Normal);
Contact contact = new Contact(geomB.WorldVertices[i], _feature.Normal, _feature.Distance, new ContactId(geomB.id, vertexIndex, geomA.id));
contactList.Add(contact);
}
}
//Lookup distancegrid B data from list
DistanceGridData geomBGridData = _distanceGrids[geomB.id];
//Iterate the first geometry vertices
for (int i = 0; i < geomA.WorldVertices.Count; i++)
{
if (contactList.Count == PhysicsSimulator.MaxContactsToDetect)
{
break;
}
vertexIndex += 1;
_vertRef = geomA.WorldVertices[i];
geomB.TransformToLocalCoordinates(ref _vertRef, out _localVertex);
if (!geomBGridData.Intersect(ref _localVertex, out _feature))
{
continue;
}
if (_feature.Distance < 0f)
{
geomB.TransformNormalToWorld(ref _feature.Normal, out _feature.Normal);
_feature.Normal = -_feature.Normal;
Contact contact = new Contact(geomA.WorldVertices[i], _feature.Normal, _feature.Distance, new ContactId(geomA.id, vertexIndex, geomB.id));
contactList.Add(contact);
}
}
}
public LineIntersectInfo(Geom geom, List <Vector2> points)
{
_geom = geom;
_points = points;
}
/// <summary>
/// Incrementally inserts the min/max extents into the ExtentList. As it
/// does so, the method ensures that overlap records, the collisionpair
/// map, and all other book-keeping is up todate.
/// </summary>
/// <param name="ourInfo">The extent info for a give axis</param>
public void IncrementalInsertExtent(ExtentInfo ourInfo)
{
Extent min = ourInfo.min;
Extent max = ourInfo.max;
Debug.Assert(min.value < max.value);
int iMin = InsertIntoSortedList(min);
int iMax = InsertIntoSortedList(max);
Geom ourGeom = ourInfo.geometry;
// As this is a newly inserted extent, we need to update the overlap
// information.
// RULE 1: Traverse from min to max. Look for other "min" Extents
// and when found, add our wrapper/geometry to their list.
int iCurr = iMin + 1;
while (iCurr != iMax)
{
if (this[iCurr].isMin)
{
this[iCurr].info.underConsideration.Add(ourGeom);
}
iCurr++;
}
// RULE 2: From min, traverse to the left until we encounter
// another "min" extent. If we find one, we add its geometry
// to our underConsideration list and go to RULE 3, otherwise
// there is no more work to do and we can exit.
iCurr = iMin - 1;
while (iCurr >= 0 && this[iCurr].isMin == false)
{
iCurr--;
}
if (iCurr < 0)
{
return;
}
List <Geom> ourUnderConsideration = ourInfo.underConsideration;
Extent currExtent = this[iCurr];
ourUnderConsideration.Add(currExtent.info.geometry);
// RULE 3: Now that we have found a "min" extent, we take
// its existing overlap list and copy it into our underConsideration
// list. All except for ourselves.
ourUnderConsideration.AddRange(currExtent.info.underConsideration);
ourUnderConsideration.Remove(ourGeom); // just in case
/*LinkedListNode<Geom> currGeomNode =
* currExtent.info.underConsideration.First;
*
* while (currGeomNode != null)
* {
* if (currGeomNode.Value != ourGeom)
* {
* ourUnderConsideration.AddLast(new LinkedListNode<Geom>(
* currGeomNode.Value));
* }
* currGeomNode = currGeomNode.Next;
* }*/
// RULE 4: Move from the found extent back toward our "min" extent.
// Whenever and "max" extent is found, we remove its reference
// from our extents list.
while (iCurr != iMin)
{
if (currExtent.isMin == false)
{
ourUnderConsideration.Remove(currExtent.info.geometry);
if (ourInfo.overlaps.Remove(currExtent.info.geometry))
{
owner.collisionPairs.RemovePair(ourGeom,
currExtent.info.geometry);
}
}
currExtent = this[++iCurr];
}
}
/// <summary>
/// This function can be used for times when frame-coherence is temporarily lost
/// or when it is simply more convenient to completely rebuild all the cached
/// data instead of incrementally updating it. Currently it is used after
/// removing disposed/removed geometries. If your application had an object
/// that teleported across the universe or some other situation where
/// frame-coherence was lost, you might consider this function.
/// </summary>
public void ForceNonIncrementalUpdate()
{
UpdateExtentValues();
// First, wipe out the collision records
collisionPairs.Clear();
// And clear out all the overlap records
Debug.Assert(_xInfoList.Count == _yInfoList.Count);
for (int i = 0; i < _xInfoList.Count; i++)
{
_xInfoList[i].overlaps.Clear();
_xInfoList[i].underConsideration.Clear();
_yInfoList[i].overlaps.Clear();
_yInfoList[i].underConsideration.Clear();
}
// Force sort
_xExtentList.Sort((l, r) => l.value.CompareTo(r.value));
_yExtentList.Sort((l, r) => l.value.CompareTo(r.value));
// Rebuild overlap information
List <Geom> overlaps = new List <Geom>();
for (int i = 0; i < 2; i++)
{
overlaps.Clear();
ExtentList extentList = i == 0 ? _xExtentList : _yExtentList;
foreach (Extent extent in extentList)
{
if (extent.isMin)
{
// Add whatever is currently in overlaps to this
extent.info.overlaps.InsertRange(0, overlaps);
// Now add, this geom to overlaps
overlaps.Add(extent.info.geometry);
}
else
{
// remove this geom from overlaps
overlaps.Remove(extent.info.geometry);
// Test this geom against its overlaps for collisionpairs
Geom thisGeom = extent.info.geometry;
foreach (Geom g in extent.info.overlaps)
{
if (DoCollision(thisGeom, g) == false)
{
continue;
}
collisionPairs.AddPair(thisGeom, g);
}
}
}
}
HandleCollisions();
}
/// <summary>
/// Updates this instance.
/// </summary>
public void Update()
{
//Iterate all the geoms and check against the next
for (int i = 0; i < _physicsSimulator.GeomList.Count - 1; i++)
{
for (int j = i + 1; j < _physicsSimulator.GeomList.Count; j++)
{
_geometryA = _physicsSimulator.GeomList[i];
_geometryB = _physicsSimulator.GeomList[j];
if (!_geometryA.body.Enabled || !_geometryB.body.Enabled)
{
continue;
}
if ((_geometryA.CollisionGroup == _geometryB.CollisionGroup) &&
_geometryA.CollisionGroup != 0 && _geometryB.CollisionGroup != 0)
{
continue;
}
if (!_geometryA.CollisionEnabled || !_geometryB.CollisionEnabled)
{
continue;
}
if (_geometryA.body.isStatic && _geometryB.body.isStatic)
{
continue;
}
if (_geometryA.body == _geometryB.body)
{
continue;
}
if (((_geometryA.CollisionCategories & _geometryB.CollidesWith) == CollisionCategory.None) &
((_geometryB.CollisionCategories & _geometryA.CollidesWith) == CollisionCategory.None))
{
continue;
}
if (_geometryA.IsGeometryIgnored(_geometryB) || _geometryB.IsGeometryIgnored(_geometryA))
{
continue;
}
//Assume intersection
bool intersection = true;
//Check if there is no intersection
if (_geometryA.AABB.min.X > _geometryB.AABB.max.X || _geometryB.AABB.min.X > _geometryA.AABB.max.X)
{
intersection = false;
}
else if (_geometryA.AABB.min.Y > _geometryB.AABB.Max.Y || _geometryB.AABB.min.Y > _geometryA.AABB.Max.Y)
{
intersection = false;
}
//Call the OnBroadPhaseCollision event first. If the user aborts the collision
//it will not create an arbiter
if (OnBroadPhaseCollision != null)
{
intersection = OnBroadPhaseCollision(_geometryA, _geometryB);
}
//If the user aborted the intersection, continue to the next geometry.
if (!intersection)
{
continue;
}
Arbiter arbiter = _physicsSimulator.arbiterPool.Fetch();
arbiter.ConstructArbiter(_geometryA, _geometryB, _physicsSimulator);
if (!_physicsSimulator.ArbiterList.Contains(arbiter))
{
_physicsSimulator.ArbiterList.Add(arbiter);
}
else
{
_physicsSimulator.arbiterPool.Insert(arbiter);
}
}
}
}
/// <summary>
/// Detects all collision points between a line and a Geom. If intersections exist a LineIntersectionInfo
/// object is created and added to an existing list of such objects.
/// </summary>
/// <param name="point1">The first point of the line segment to test</param>
/// <param name="point2">The second point of the line segment to test</param>
/// <param name="geom">The geometry to test.</param>
/// <param name="detectUsingAABB">If true, intersection will be tested using the Geoms AABB. If false, the Geoms vertices will be used.</param>
/// <param name="intersectionPoints">An existing point list to add to</param>
public static void LineSegmentGeomIntersect(ref Vector2 point1, ref Vector2 point2, Geom geom, bool detectUsingAABB,
ref List <Vector2> intersectionPoints)
{
if (detectUsingAABB)
{
LineSegmentAABBIntersect(ref point1, ref point2, geom.AABB, ref intersectionPoints);
}
else
{
LineSegmentVerticesIntersect(ref point1, ref point2, geom.WorldVertices, ref intersectionPoints);
}
}
/// <summary>
/// Test AABB collisions between two geometries. Tests include checking if the
/// geometries are enabled, static, in the right collision categories, etc.
/// </summary>
/// <returns>Returns true if there is a collision, false otherwise</returns>
public static bool DoCollision(Geom g1, Geom g2)
{
if (!g1.body.enabled || !g2.body.enabled)
{
return(false);
}
if ((g1.collisionGroup == g2.collisionGroup) &&
g1.collisionGroup != 0 && g2.collisionGroup != 0)
{
return(false);
}
if (!g1.collisionEnabled || !g2.collisionEnabled)
{
return(false);
}
if (g1.body.isStatic && g2.body.isStatic)
{
return(false);
}
if (g1.body == g2.body)
{
return(false);
}
if (((g1.collisionCategories & g2.collidesWith) ==
CollisionCategories.None) & ((g2.collisionCategories &
g1.collidesWith) == CollisionCategories.None))
{
return(false);
}
//TMP
AABB aabb1 = new AABB();
AABB aabb2 = new AABB();
aabb1.min = g1.aabb.min;
aabb1.max = g1.aabb.max;
aabb2.min = g2.aabb.min;
aabb2.max = g2.aabb.max;
aabb1.min.X -= fTol;
aabb1.min.Y -= fTol;
aabb1.max.X += fTol;
aabb1.max.Y += fTol;
aabb2.min.X -= fTol;
aabb2.min.Y -= fTol;
aabb2.max.X += fTol;
aabb2.max.Y += fTol;
if (AABB.Intersect(aabb1, aabb2) == false)
{
return(false);
}
// if (AABB.Intersect(g1.aabb, g2.aabb) == false)
// return false;
return(true);
}
private void DoCollision()
{
for (int i = 0; i < _physicsSimulator.geomList.Count - 1; i++)
{
for (int j = i + 1; j < _physicsSimulator.geomList.Count; j++)
{
_geometryA = _physicsSimulator.geomList[i];
_geometryB = _physicsSimulator.geomList[j];
//possible early exits
if (!_geometryA.Body.enabled || !_geometryB.Body.enabled)
{
continue;
}
if ((_geometryA.CollisionGroup == _geometryB.CollisionGroup) && _geometryA.CollisionGroup != 0 &&
_geometryB.CollisionGroup != 0)
{
continue;
}
if (!_geometryA.CollisionEnabled || !_geometryB.CollisionEnabled)
{
continue;
}
if (_geometryA.Body.isStatic && _geometryB.Body.isStatic)
{
//don't collide two static bodies
continue;
}
if (_geometryA.Body == _geometryB.Body)
{
//don't collide two geometries connected to the same body
continue;
}
if (((_geometryA.CollisionCategories & _geometryB.CollidesWith) == CollisionCategories.None) &
((_geometryB.CollisionCategories & _geometryA.CollidesWith) == CollisionCategories.None))
{
continue;
}
bool intersection = true;
#region INLINE: if (AABB.Intersect(_geometryA.aabb, _geometryB.aabb)) ....
if (_geometryA.AABB.min.X > _geometryB.AABB.max.X || _geometryB.AABB.min.X > _geometryA.AABB.max.X)
{
intersection = false;
}
else if (_geometryA.AABB.min.Y > _geometryB.AABB.Max.Y ||
_geometryB.AABB.min.Y > _geometryA.AABB.Max.Y)
{
intersection = false;
}
#endregion
if (intersection)
{
_arbiter = _physicsSimulator.arbiterPool.Fetch();
_arbiter.ConstructArbiter(_geometryA, _geometryB, _physicsSimulator);
if (!_physicsSimulator.arbiterList.Contains(_arbiter))
{
_physicsSimulator.arbiterList.Add(_arbiter);
}
else
{
_physicsSimulator.arbiterPool.Release(_arbiter);
}
}
}
}
}
/// <summary>
/// Detects all collision points between a line and a Geom. If intersections exist
/// a LineIntersectionInfo object is created and added to an existing list of such
/// objects. </summary>
/// <param name="point1">The first point of the line segment to test</param>
/// <param name="point2">The second point of the line segment to test</param>
/// <param name="geom">The geometry to test.</param>
/// <param name="detectUsingAABB">If true, intersection will be tested using the
/// Geoms AABB. If false, the Geoms vertices will be used.</param>
/// <param name="intersectionPoints">An existing points info list to add to
/// </param>
public static void LineSegmentGeomIntersect(Vector2 point1, Vector2 point2, Geom geom, bool detectUsingAABB,
ref List <Vector2> intersectionPoints)
{
LineSegmentGeomIntersect(ref point1, ref point2, geom, detectUsingAABB, ref intersectionPoints);
}
/// <summary>
/// Detects all collision points between a line and a Geom. If intersections exist
/// a LineIntersectionInfo object is created and added to an existing list of such
/// objects. </summary>
/// <param name="p1">The first point of the line segment to test</param>
/// <param name="p2">The second point of the line segment to test</param>
/// <param name="geom">The geometry to test.</param>
/// <param name="detectUsingAABB">If true, intersection will be tested using the
/// Geoms AABB. If false, the Geoms vertices will be used.</param>
/// <param name="lineIntersectInfoList">An existing intersect info list to add to
/// </param>
public static void LineSegmentGeomIntersect(Vector2 p1, Vector2 p2, Geom geom, bool detectUsingAABB,
ref List <LineIntersectInfo> lineIntersectInfoList)
{
LineSegmentGeomIntersect(ref p1, ref p2, geom, detectUsingAABB, ref lineIntersectInfoList);
}
public bool Intersect(Geom geom, ref Vector2 position)
{
return(InsidePolygon(geom.LocalVertices, position));
}
private void DetectInternal(bool doX)
{
List <Stub> stubs = (doX) ? (_xStubs) : (_yStubs);
_currentBodies.Clear();
for (int index = 0; index < stubs.Count; index++)
{
Stub stub = stubs[index];
Wrapper wrapper1 = stub.Wrapper;
if (stub.Begin)
{
//set the min and max values
if (doX)
{
wrapper1.SetY();
}
else
{
wrapper1.SetX();
}
Geom geometryA = wrapper1.Geom;
for (LinkedListNode <Wrapper> node = _currentBodies.First; node != null; node = node.Next)
{
Wrapper wrapper2 = node.Value;
Geom geometryB = wrapper2.Geom;
if (wrapper1.Min <= wrapper2.Max && wrapper2.Min <= wrapper1.Max)
{
if (!geometryA.body.Enabled || !geometryB.body.Enabled)
{
continue;
}
if ((geometryA.CollisionGroup == geometryB.CollisionGroup) &&
geometryA.CollisionGroup != 0 && geometryB.CollisionGroup != 0)
{
continue;
}
if (!geometryA.CollisionEnabled || !geometryB.CollisionEnabled)
{
continue;
}
if (geometryA.body.isStatic && geometryB.body.isStatic)
{
continue;
}
if (geometryA.body == geometryB.body)
{
continue;
}
if (((geometryA.CollisionCategories & geometryB.CollidesWith) == CollisionCategory.None) &
((geometryB.CollisionCategories & geometryA.CollidesWith) == CollisionCategory.None))
{
continue;
}
if (geometryA.IsGeometryIgnored(geometryB) || geometryB.IsGeometryIgnored(geometryA))
{
continue;
}
bool intersection = true;
//Call the OnBroadPhaseCollision event first. If the user aborts the collision
//it will not create an arbiter
if (OnBroadPhaseCollision != null)
{
intersection = OnBroadPhaseCollision(geometryA, geometryB);
}
if (!intersection)
{
continue;
}
_physicsSimulator.ArbiterList.AddArbiterForGeomPair(_physicsSimulator, geometryA, geometryB);
}
}
if (wrapper1.ShouldAddNode)
{
_currentBodies.AddLast(wrapper1.Node);
}
}
else
{
if (wrapper1.ShouldAddNode)
{
_currentBodies.Remove(wrapper1.Node);
}
}
}
}
private void RunHash()
{
_keysToRemove.Clear();
foreach (KeyValuePair <long, List <Geom> > pair in _hash)
{
// If there are no geometries in the list. Remove it.
// If there are any geometries in the list, process them.
List <Geom> list = pair.Value;
if (list.Count == 0)
{
_keysToRemove.Add(pair.Key);
}
else
{
for (int i = 0; i < list.Count - 1; i++)
{
Geom geometryA = list[i];
for (int j = i + 1; j < list.Count; j++)
{
Geom geometryB = list[j];
if (!geometryA.body.Enabled || !geometryB.body.Enabled)
{
continue;
}
if ((geometryA.CollisionGroup == geometryB.CollisionGroup) &&
geometryA.CollisionGroup != 0 && geometryB.CollisionGroup != 0)
{
continue;
}
if (!geometryA.CollisionEnabled || !geometryB.CollisionEnabled)
{
continue;
}
if (geometryA.body.isStatic && geometryB.body.isStatic)
{
continue;
}
if (geometryA.body == geometryB.body)
{
continue;
}
if (((geometryA.CollisionCategories & geometryB.CollidesWith) == CollisionCategory.None) &
((geometryB.CollisionCategories & geometryA.CollidesWith) == CollisionCategory.None))
{
continue;
}
if (geometryA.IsGeometryIgnored(geometryB) || geometryB.IsGeometryIgnored(geometryA))
{
continue;
}
long key = PairID.GetId(geometryA.id, geometryB.id);
if (!_filter.ContainsKey(key))
{
_filter.Add(key, null);
//Check if there is intersection
bool intersection = AABB.Intersect(ref geometryA.AABB, ref geometryB.AABB);
//User can cancel collision
if (OnBroadPhaseCollision != null)
{
intersection = OnBroadPhaseCollision(geometryA, geometryB);
}
if (!intersection)
{
continue;
}
_physicsSimulator.ArbiterList.AddArbiterForGeomPair(_physicsSimulator, geometryA, geometryB);
}
}
}
list.Clear();
}
}
_filter.Clear();
//Remove all the empty lists from the hash
for (int index = 0; index < _keysToRemove.Count; ++index)
{
_hash.Remove(_keysToRemove[index]);
}
}
public void MoveUnderConsiderationToOverlaps()
{
for (int i = 0; i < Count; i++)
{
if (this[i].UnderConsideration.Count == 0)
{
continue;
}
Geom geometryA = this[i].Geometry;
// First transfer those under consideration to overlaps,
// for, they have been considered...
int startIndex = this[i].Overlaps.Count;
this[i].Overlaps.AddRange(this[i].UnderConsideration);
this[i].UnderConsideration.Clear();
for (int j = startIndex; j < this[i].Overlaps.Count; j++)
{
Geom geometryB = this[i].Overlaps[j];
// It is possible that we may test the same pair of geometries
// for both extents (x and y), however, I'm banking on that
// one of the extents has probably already been cached and
// therefore, won't be checked.
if (!geometryA.body.Enabled || !geometryB.body.Enabled)
{
continue;
}
if ((geometryA.CollisionGroup == geometryB.CollisionGroup) &&
geometryA.CollisionGroup != 0 && geometryB.CollisionGroup != 0)
{
continue;
}
if (!geometryA.CollisionEnabled || !geometryB.CollisionEnabled)
{
continue;
}
if (geometryA.body.isStatic && geometryB.body.isStatic)
{
continue;
}
if (geometryA.body == geometryB.body)
{
continue;
}
if (((geometryA.CollisionCategories & geometryB.CollidesWith) == CollisionCategory.None) &
((geometryB.CollisionCategories & geometryA.CollidesWith) == CollisionCategory.None))
{
continue;
}
if (geometryA.IsGeometryIgnored(geometryB) || geometryB.IsGeometryIgnored(geometryA))
{
continue;
}
//TMP
AABB aabb1 = new AABB();
AABB aabb2 = new AABB();
aabb1.min = geometryA.AABB.min;
aabb1.max = geometryA.AABB.max;
aabb2.min = geometryB.AABB.min;
aabb2.max = geometryB.AABB.max;
aabb1.min.X -= _floatTolerance;
aabb1.min.Y -= _floatTolerance;
aabb1.max.X += _floatTolerance;
aabb1.max.Y += _floatTolerance;
aabb2.min.X -= _floatTolerance;
aabb2.min.Y -= _floatTolerance;
aabb2.max.X += _floatTolerance;
aabb2.max.Y += _floatTolerance;
if (!AABB.Intersect(ref aabb1, ref aabb2))
{
continue;
}
//Call the OnBroadPhaseCollision event first. If the user aborts the collision
//it will not create an arbiter
if (Owner.OnBroadPhaseCollision != null)
{
if (Owner.OnBroadPhaseCollision(geometryA, geometryB))
{
Owner.CollisionPairs.AddPair(geometryA, geometryB);
}
}
else
{
Owner.CollisionPairs.AddPair(geometryA, geometryB);
}
}
}
}
/// <summary>
/// Returns the contact list from two possibly intersecting Geom's.
/// This is the stationary version of this function. It doesn't
/// account for linear or angular motion.
/// </summary>
/// <param name="geomA">The first Geom.</param>
/// <param name="geomB">The second Geom.</param>
/// <param name="contactList">Set of Contacts between the two Geoms.
/// NOTE- this will be empty if no contacts are present.</param>
public void Collide(Geom geomA, Geom geomB, ContactList contactList)
{
PolygonCollisionResult result = PolygonCollision(geomA.WorldVertices, geomB.WorldVertices);
float distance = result.MinimumTranslationVector.Length();
int contactsDetected = 0;
Vector2 normal = Vector2.Normalize(-result.MinimumTranslationVector);
int contactsHandled = 0;
if (result.Intersect && distance > 0.001f)
{
for (int i = 0; i < geomA.WorldVertices.Count; i++)
{
if (contactsDetected <= PhysicsSimulator.MaxContactsToDetect)
{
if (InsidePolygon(geomB.WorldVertices, geomA.WorldVertices[i]))
{
Contact c = new Contact(geomA.WorldVertices[i], normal, -distance, new ContactId(geomA.id, i, geomB.id));
contactList.Add(c);
contactsDetected++;
contactsHandled++;
}
}
else
{
break;
}
}
contactsDetected = 0;
for (int i = 0; i < geomB.WorldVertices.Count; i++)
{
if (contactsDetected <= PhysicsSimulator.MaxContactsToDetect)
{
if (InsidePolygon(geomA.WorldVertices, geomB.WorldVertices[i]))
{
Contact c = new Contact(geomB.WorldVertices[i], normal, -distance, new ContactId(geomB.id, i, geomA.id));
contactList.Add(c);
contactsDetected++;
contactsHandled++;
}
}
else
{
break;
}
}
// No vertices of either polygon are inside the other, despite their intersection.
// (Think of an X made of two rectangles of four vertices each.)
// So select the vertex that is furthest past the edge forming the
// separating axis as the contact point.
// - Andrew Russell
if (contactsHandled == 0)
{
int edgeIndex = result.bestEdgeIndex;
Geom separatingEdgeOn = geomA;
Geom otherPolygon = geomB;
if (result.bestEdgeIndex >= geomA.WorldVertices.Count)
{
edgeIndex -= geomA.WorldVertices.Count;
separatingEdgeOn = geomB;
otherPolygon = geomA;
}
Vector2 edge = separatingEdgeOn.WorldVertices.GetEdge(edgeIndex);
Vector2 axis = new Vector2(-edge.Y, edge.X);
axis.Normalize();
int mostPenetrationIndex = 0;
float mostPenetration = Vector2.Dot(axis, otherPolygon.WorldVertices[0]);
for (int i = 1; i < otherPolygon.WorldVertices.Count; i++)
{
float penetration = Vector2.Dot(axis, otherPolygon.WorldVertices[i]);
if (penetration < mostPenetration)
{
mostPenetration = penetration;
mostPenetrationIndex = i;
}
}
Contact c = new Contact(otherPolygon.WorldVertices[mostPenetrationIndex], normal, -distance,
new ContactId(otherPolygon.id, mostPenetrationIndex, separatingEdgeOn.id));
contactList.Add(c);
}
}
}
///<summary>
/// Not required by brute force collider
///</summary>
public void Add(Geom geom)
{
}
private void DoCollision()
{
//Iterate all the geoms and check against the next
for (int i = 0; i < _physicsSimulator.geomList.Count - 1; i++)
{
for (int j = i + 1; j < _physicsSimulator.geomList.Count; j++)
{
_geometryA = _physicsSimulator.geomList[i];
_geometryB = _physicsSimulator.geomList[j];
if (!_geometryA.body.Enabled || !_geometryB.body.Enabled)
{
continue;
}
if ((_geometryA.collisionGroup == _geometryB.collisionGroup) &&
_geometryA.collisionGroup != 0 && _geometryB.collisionGroup != 0)
{
continue;
}
if (!_geometryA.collisionEnabled || !_geometryB.collisionEnabled)
{
continue;
}
if (_geometryA.body.isStatic && _geometryB.body.isStatic)
{
continue;
}
if (_geometryA.body == _geometryB.body)
{
continue;
}
if (((_geometryA.CollisionCategories & _geometryB.CollidesWith) == CollisionCategory.None) &
((_geometryB.CollisionCategories & _geometryA.CollidesWith) == CollisionCategory.None))
{
continue;
}
//Assume intersection
bool intersection = true;
#region INLINE: if (AABB.Intersect(_geometryA.aabb, _geometryB.aabb)) ....
//Check if there is no intersection
if (_geometryA.AABB.min.X > _geometryB.AABB.max.X || _geometryB.AABB.min.X > _geometryA.AABB.max.X)
{
intersection = false;
}
else if (_geometryA.AABB.min.Y > _geometryB.AABB.Max.Y ||
_geometryB.AABB.min.Y > _geometryA.AABB.Max.Y)
{
intersection = false;
}
#endregion
//Call the OnBroadPhaseCollision event first. If the user aborts the collision
//it will not create an arbiter
if (OnBroadPhaseCollision != null)
{
intersection = OnBroadPhaseCollision(_geometryA, _geometryB);
}
//If the user aborted the intersection, continue to the next geometry.
if (!intersection)
{
continue;
}
//Note: Commented this out and copy-paste into from other colliders
//_arbiter = _physicsSimulator.arbiterPool.Fetch();
//_arbiter.ConstructArbiter(_geometryA, _geometryB, _physicsSimulator);
//if (_physicsSimulator.arbiterList.Contains(_arbiter))
//{
// _physicsSimulator.arbiterPool.Insert(_arbiter);
//}
//else
//{
// _physicsSimulator.arbiterList.Add(_arbiter);
//}
Arbiter arbiter = _physicsSimulator.arbiterPool.Fetch();
arbiter.ConstructArbiter(_geometryA, _geometryB, _physicsSimulator);
if (!_physicsSimulator.arbiterList.Contains(arbiter))
{
_physicsSimulator.arbiterList.Add(arbiter);
}
else
{
_physicsSimulator.arbiterPool.Insert(arbiter);
}
}
}
}
public void Add(Geom geom)
{
//not
}
