/// <summary>
/// Checks if the specified geom intersect the specified point.
/// </summary>
/// <param name="geom">The geom.</param>
/// <param name="point">The point.</param>
/// <returns></returns>
public bool Intersect(Geom geom, ref Vector2 point)
{
//Lookup the geometry's distancegrid
DistanceGridData gridData = _distanceGrids[geom.id];
Feature feature;
return(gridData.Intersect(ref point, out feature));
}
/// <summary>
/// Computes the grid.
/// </summary>
/// <param name="geom">The geometry.</param>
/// <exception cref="ArgumentNullException"><c>geometry</c> is null.</exception>
public void CreateDistanceGrid(Geom geom)
{
if (geom == null)
{
throw new ArgumentNullException("geom", "Geometry can't be null");
}
//Don't create distancegrid for geometry that already have one.
//NOTE: This should be used to -update- the geometry's distance grid (if grid cell size has changed).
if (_distanceGrids.ContainsKey(geom.id))
{
return;
}
//By default, calculate the gridcellsize from the AABB
if (geom.GridCellSize <= 0)
{
geom.GridCellSize = CalculateGridCellSizeFromAABB(ref geom.AABB);
}
//Prepare the geometry. Reset the geometry matrix
Matrix old = geom.Matrix;
geom.Matrix = Matrix.Identity;
//Create data needed for gridcalculations
AABB aabb = new AABB(ref geom.AABB);
float gridCellSizeInv = 1 / geom.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;
float[,] nodes = new float[xSize, ySize];
Vector2 vector = aabb.Min;
for (int x = 0; x < xSize; ++x, vector.X += geom.GridCellSize)
{
vector.Y = aabb.Min.Y;
for (int y = 0; y < ySize; ++y, vector.Y += geom.GridCellSize)
{
nodes[x, y] = geom.GetNearestDistance(ref vector); // shape.GetDistance(vector);
}
}
//restore the geometry
geom.Matrix = old;
DistanceGridData distanceGridData = new DistanceGridData();
distanceGridData.AABB = aabb;
distanceGridData.GridCellSize = geom.GridCellSize;
distanceGridData.GridCellSizeInv = gridCellSizeInv;
distanceGridData.Nodes = nodes;
_distanceGrids.Add(geom.id, distanceGridData);
}
/// <summary>
/// Computes the grid.
/// </summary>
/// <param name="geom">The geometry.</param>
/// <exception cref="ArgumentNullException"><c>geometry</c> is null.</exception>
public void CreateDistanceGrid(Geom geom)
{
if (geom == null)
throw new ArgumentNullException("geom", "Geometry can't be null");
//Don't create distancegrid for geometry that already have one.
//NOTE: This should be used to -update- the geometry's distance grid (if grid cell size has changed).
if (_distanceGrids.ContainsKey(geom.id))
return;
//By default, calculate the gridcellsize from the AABB
if (geom.GridCellSize <= 0)
geom.GridCellSize = CalculateGridCellSizeFromAABB(ref geom.AABB);
//Prepare the geometry. Reset the geometry matrix
Matrix old = geom.Matrix;
geom.Matrix = Matrix.Identity;
//Create data needed for gridcalculations
AABB aabb = new AABB(ref geom.AABB);
float gridCellSizeInv = 1 / geom.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;
float[,] nodes = new float[xSize, ySize];
Vector2 vector = aabb.Min;
for (int x = 0; x < xSize; ++x, vector.X += geom.GridCellSize)
{
vector.Y = aabb.Min.Y;
for (int y = 0; y < ySize; ++y, vector.Y += geom.GridCellSize)
{
nodes[x, y] = geom.GetNearestDistance(ref vector); // shape.GetDistance(vector);
}
}
//restore the geometry
geom.Matrix = old;
DistanceGridData distanceGridData = new DistanceGridData();
distanceGridData.AABB = aabb;
distanceGridData.GridCellSize = geom.GridCellSize;
distanceGridData.GridCellSizeInv = gridCellSizeInv;
distanceGridData.Nodes = nodes;
_distanceGrids.Add(geom.id, distanceGridData);
}
/// <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);
}
}
}
/// <summary>
/// Clones this instance.
/// </summary>
/// <returns></returns>
public DistanceGridData Clone()
{
DistanceGridData grid = new DistanceGridData();
grid.GridCellSize = GridCellSize;
grid.GridCellSizeInv = GridCellSizeInv;
grid.AABB = AABB;
grid.Nodes = (float[,])Nodes.Clone();
return grid;
}
