public SatResult? FindIntersection(Geometry a, Geometry b)
{
var aAxes = a.GetAxes(b);
var bAxes = b.GetAxes(a);
var totalAxes = aAxes.Length + bAxes.Length;
Vector2 normal = Vector2.Zero;
Vector2 deepestPoint = Vector2.Zero;
float smallestOverlap = float.MaxValue;
for (int i = 0; i < totalAxes; i++)
{
Vector2 axis;
if (i < aAxes.Length)
axis = aAxes[i];
else
axis = bAxes[i - aAxes.Length];
Vector2.Normalize(ref axis, out axis);
var aProjection = a.Project(axis);
var bProjection = b.Project(axis);
float overlap;
int order;
if (!aProjection.Overlaps(bProjection, out overlap, out order))
return null;
if (overlap < smallestOverlap)
{
smallestOverlap = overlap;
normal = axis;
if (order < 0)
normal = -normal;
if (i < aAxes.Length)
{
if (order > 0) // if a is before b
deepestPoint = bProjection.StartPoint;
else
deepestPoint = bProjection.EndPoint;
}
else
{
if (order > 0)
deepestPoint = aProjection.EndPoint;
else
deepestPoint = aProjection.StartPoint;
}
}
}
var axisMagnitude = normal.Length();
normal /= axisMagnitude;
smallestOverlap /= axisMagnitude;
return new SatResult(a, b, normal, smallestOverlap, deepestPoint);
}
public SatResult(Geometry a, Geometry b, Vector2 normalAxis, float penetration, Vector2 deepestPoint)
{
A = a;
B = b;
NormalAxis = normalAxis;
Penetration = penetration;
DeepestPoint = deepestPoint;
}
public override Vector2[] GetAxes(Geometry otherObject)
{
_axes[0] = otherObject.GetClosestVertex(_transformedCentre) - _transformedCentre;
if (_axes[0] == Vector2.Zero)
_axes[0] = Vector2.UnitY;
return _axes;
}
public Contact(Vector2 position, Geometry geometry, int feature)
{
Position = position;
ID = new ContactID(geometry, feature);
MassNormal = 0;
MassTangent = 0;
NormalVelocityBias = 0;
BounceVelocity = 0;
NormalImpulse = 0;
TangentImpulse = 0;
NormalImpulseBias = 0;
}
public bool Remove(Geometry geom)
{
if (_geometry.Remove(geom))
{
for (int i = _collisions.Count - 1; i >= 0; i--)
{
var collision = _collisions[i];
if (collision.A == geom || collision.B == geom)
{
collision.Dispose();
_collisions.RemoveAt(i);
}
}
return true;
}
return false;
}
public CollisionImpulseApplied(Geometry a, Geometry b, Vector2 impulse)
{
A = a;
B = b;
Impulse = impulse;
}
public static Collision Create(Geometry a, Geometry b)
{
var collision = _pool.Count > 0 ? _pool.Pop() : new Collision();
collision.A = a;
collision.B = b;
return collision;
}
public void Add(Geometry geom)
{
_geometry.Add(geom);
}
public ContactID(Geometry geometry, int feature)
{
Geometry = geometry;
Feature = feature;
}
public override Vector2[] GetAxes(Geometry otherObject)
{
return _worldAxes;
}
/// <summary> /// Gets an array of axes, each pointing out from each face on this geometry. /// </summary> /// <param name="otherObject">The other geometry instance this instance is to be tested against.</param> /// <returns>An array of axes, each pointing out from each face on this geometry.</returns> public abstract Vector2[] GetAxes(Geometry otherObject);
