private Rectangle _GetNearbyTiles(float x, float y, float width, float height, float pad)
{
// use offset from tile map center.
x -= _position.X;
y -= _position.Y;
if (Rotation != 0.0f)
{
// need to transform world-space box to tile map aligned box which includes
// original box.
Rotation2D rotation = new Rotation2D(MathHelper.ToRadians(Rotation));
// figure tile space position
Vector2 newPos = new Vector2(x, y);
newPos = rotation.Unrotate(newPos);
x = newPos.X;
y = newPos.Y;
// compute tile space width/height, expanded to include all
// of original box in tile aligned box
Vector2 xvec = rotation.X;
Vector2 yvec = rotation.Y;
float newWidth = width * Math.Abs(xvec.X) + height * Math.Abs(xvec.Y);
float newHeight = width * Math.Abs(yvec.X) + height * Math.Abs(yvec.Y);
width = newWidth;
height = newHeight;
}
float minX, minY, maxX, maxY;
minX = ((x - (width * 0.5f)) / _tileSize.X) + (_mapSize.X * 0.5f);
maxX = ((x + (width * 0.5f)) / _tileSize.X) + (_mapSize.X * 0.5f);
minY = ((y - (height * 0.5f)) / _tileSize.Y) + (_mapSize.Y * 0.5f);
maxY = ((y + (height * 0.5f)) / _tileSize.Y) + (_mapSize.Y * 0.5f);
minX -= pad;
minY -= pad;
maxX += pad;
maxY += pad;
// make sure that returned rectangle only contains valid tile boundaries
minX = MathHelper.Clamp(minX, 0.0f, _mapSize.X - 1);
maxX = MathHelper.Clamp(maxX + 1, 0.0f, _mapSize.X - 1);
minY = MathHelper.Clamp(minY, 0.0f, _mapSize.Y - 1);
maxY = MathHelper.Clamp(maxY + 1, 0.0f, _mapSize.Y - 1);
return new Rectangle((int)minX, (int)minY, (int)(maxX), (int)(maxY));
}
public static bool IntersectMovingPolyPoly(
float elapsedTime, Vector2[] srcVertexList, Vector2[] dstVertexList,
Vector2 srcPosition, Vector2 dstPosition,
float srcRotation, float dstRotation,
Vector2 srcVelocity, Vector2 dstVelocity,
ref Vector2 collisionPosition, ref Vector2 collisionNormal, ref Vector2 collisionPenetration, ref float time)
{
if (srcVertexList == null || dstVertexList == null)
return false;
int srcVertexCount = srcVertexList.Length;
int dstVertexCount = dstVertexList.Length;
//src
Rotation2D srcRot = new Rotation2D(MathHelper.ToRadians(srcRotation));
//dst
Rotation2D dstInverseRot = new Rotation2D(MathHelper.ToRadians(-dstRotation));
Rotation2D refLocalRot = srcRot * dstInverseRot;
Vector2 offset = srcPosition - dstPosition;
Vector2 refLocalOffset = dstInverseRot.Rotate(offset);
Vector2 velocity = srcVelocity - dstVelocity;
Vector2 refLocalVelocity = dstInverseRot.Rotate(velocity);
float refVelSqr = Vector2.Dot(refLocalVelocity, refLocalVelocity);
int axesCount = 0;
float fullTimeStep = elapsedTime;
float Epsilon = 0.0001f;
// Ignore small velocities!
if (refVelSqr > Epsilon)
{
// Set Axis.
_vertexAxis[axesCount] = new Vector2(-refLocalVelocity.Y, refLocalVelocity.X);
// Check Interval Intersection.
if (!_CheckIntervalIntersection(
srcVertexList,
dstVertexList,
ref _vertexAxis[axesCount],
ref refLocalOffset, ref refLocalVelocity, ref refLocalRot,
ref _timeAxis[axesCount], fullTimeStep))
// No Collision!
return false;
// Next Axes.
axesCount++;
}
// Test Seperation Axes for Source Object.
// NOTE:- We ignore if it's a point!
if (srcVertexCount > 1)
{
for (int j = srcVertexCount - 1, i = 0; i < srcVertexCount; j = i, i++)
{
// fetch the edge
Vector2 dP = srcVertexList[i] - srcVertexList[j];
Vector2 dP2 = new Vector2(-dP.Y, dP.X);
_vertexAxis[axesCount] = refLocalRot.Rotate(dP2);
// check interval intersection
if (!_CheckIntervalIntersection(srcVertexList, dstVertexList, ref _vertexAxis[axesCount], ref refLocalOffset, ref refLocalVelocity, ref refLocalRot, ref _timeAxis[axesCount], fullTimeStep))
return false;
//next axes
axesCount++;
}
}
// Test Seperation Axes for Destination Object.
// NOTE:- We ignore if it's a point!
if (dstVertexCount > 1)
{
for (int j = dstVertexCount - 1, i = 0; i < dstVertexCount; j = i, i++)
{
// fetch the edge
Vector2 dP = dstVertexList[i] - dstVertexList[j];
// set the axis
_vertexAxis[axesCount] = new Vector2(-dP.Y, dP.X);
if (!_CheckIntervalIntersection(srcVertexList, dstVertexList, ref _vertexAxis[axesCount], ref refLocalOffset, ref refLocalVelocity, ref refLocalRot, ref _timeAxis[axesCount], fullTimeStep))
return false;
//next axes
axesCount++;
}
}
// Test Special-Case for Segments for Destination Object.
if (dstVertexCount == 2)
{
// Set Axis.
_vertexAxis[axesCount] = (dstVertexList[1] - dstVertexList[0]);
// Check Interval Intersection.
if (!_CheckIntervalIntersection(srcVertexList, dstVertexList, ref _vertexAxis[axesCount], ref refLocalOffset, ref refLocalVelocity, ref refLocalRot, ref _timeAxis[axesCount], fullTimeStep))
// No Collision!
return false;
// Next Axes.
axesCount++;
}
// Test Special-Case for Segments for Source Object.
if (srcVertexCount == 2)
{
// Set Axis.
Vector2 segment = (srcVertexList[1] - srcVertexList[0]);
_vertexAxis[axesCount] = refLocalRot.Rotate(segment);
// Check Interval Intersection.
if (!_CheckIntervalIntersection(srcVertexList, dstVertexList, ref _vertexAxis[axesCount], ref refLocalOffset, ref refLocalVelocity, ref refLocalRot, ref _timeAxis[axesCount], fullTimeStep))
// No Collision!
return false;
// Next Axes.
axesCount++;
}
// find minimum seperation distance
if (!_FindCollisionTime(_vertexAxis, _timeAxis, axesCount, ref collisionNormal, ref time))
return false;
// Respace Normal.
collisionNormal = dstInverseRot.Unrotate(collisionNormal);
// need this below (simple transpose)
Rotation2D dstRot = dstInverseRot.Invert();
// compute offset between poly centers for following calculation
Vector2 polyCenter = new Vector2();
foreach (Vector2 v in srcVertexList)
polyCenter += v;
polyCenter *= 1.0f / (float)srcVertexList.Length;
Vector2 polyOffset = srcPosition + srcRot.Rotate(polyCenter);
polyCenter = new Vector2();
foreach (Vector2 v in dstVertexList)
polyCenter += v;
polyCenter *= 1.0f / (float)dstVertexList.Length;
polyOffset -= dstPosition + dstRot.Rotate(polyCenter);
// Make sure the collision polygons are pushed away.
// NOTE: This is the overlap case.
if (Vector2.Dot(collisionNormal, polyOffset) < 0.0f)
collisionNormal = -collisionNormal;
int contactCount = 0;
if (!_FindContactPoints(
srcVertexList, srcPosition, srcVelocity, ref srcRot,
dstVertexList, dstPosition, dstVelocity, ref dstRot,
collisionNormal, time, _srcContacts, _dstContacts, out contactCount))
// No Support Points!
return false;
collisionPosition = 0.5f * (_srcContacts[0] + _dstContacts[0]);
collisionPenetration = _dstContacts[0] - _srcContacts[0];
if (contactCount == 2)
{
collisionPosition = 0.5f * collisionPosition + 0.25f * (_srcContacts[1] + _dstContacts[1]);
Vector2 collisionPenetration2 = _dstContacts[1] - _srcContacts[1];
if (collisionPenetration2.LengthSquared() > collisionPenetration.LengthSquared())
collisionPenetration = collisionPenetration2;
}
// return true to indicate collision
return true;
}
//-----------------------------------------------------------------------------------------------
// Find Support Points.
// NOTE:- This is a convex shape along a specified direction.
//-----------------------------------------------------------------------------------------------
static int _FindSupportPoints(
Vector2[] poly, Vector2 position, Vector2 velocity, ref Rotation2D rotation,
Vector2 collisionNormal, float collisionTime, Vector2[] supportPoints)
{
// Calculate Normal.
Vector2 normal = rotation.Unrotate(collisionNormal);
// Reset Direction.
float supportMin = _support[0] = Vector2.Dot(poly[0], normal);
// Interate Polygon.
for (int i = 1; i < poly.Length; i++)
{
// Calculate.
_support[i] = Vector2.Dot(poly[i], normal);
// Check For Minimum.
if (_support[i] < supportMin)
supportMin = _support[i];
}
// The limit here is two support-points only.
// If we find more then we use the extremums.
int supportCount = 0;
// Set Threshold.
float threshold = 1.0e-3f;
// Reset Sign Flag.
bool sign = false;
// Calculate Perpendicular Normal.
Vector2 perpNormal = new Vector2(-collisionNormal.Y, collisionNormal.X);
// Interate Polygon.
for (int i = 0; i < poly.Length; i++)
{
// Check Contact.
if (_support[i] < supportMin + threshold)
{
// Transform Contact to World-Space.
Vector2 contact;
_TransformContact(poly[i], position, velocity, ref rotation, collisionTime, out contact);
// Contact Dot.
float contactDot = Vector2.Dot(contact, perpNormal);
// Less than two supports?
if (supportCount < 2)
{
// Yes, so note contact.
_supportMinMax[supportCount] = contactDot;
supportPoints[supportCount] = contact;
// Increase Support Count.
supportCount++;
// Note Sign for two contacts.
if (supportCount > 1)
sign = (_supportMinMax[1] > _supportMinMax[0]);
}
else
{
int idx0 = 0;
int idx1 = 1;
if (!sign)
TorqueUtil.Swap(ref idx0, ref idx1);
if (contactDot < _supportMinMax[idx0])
{
_supportMinMax[idx0] = contactDot;
supportPoints[idx0] = contact;
}
else if (contactDot > _supportMinMax[idx1])
{
_supportMinMax[idx1] = contactDot;
supportPoints[idx1] = contact;
}
}
}
}
// Return Support Count.
return supportCount;
}
static bool _CheckIntervalIntersection(Vector2[] srcVertexList, Vector2[] dstVertexList, ref Vector2 vertexAxis, ref Vector2 refLocalOffset, ref Vector2 refLocalVelocity, ref Rotation2D refLocalRot, ref float timeAxis, float collisionTime)
{
// calculate intervals for source/destination
Vector2 vertexAxisRotated = refLocalRot.Unrotate(vertexAxis);
// Get the interval of the polygons projected onto the axis. Min returned in x, max in y.
Vector2 srcProj = _CalculateInterval(srcVertexList, ref vertexAxisRotated);
Vector2 dstProj = _CalculateInterval(dstVertexList, ref vertexAxis);
// add reference offset
float srcOffset = Vector2.Dot(refLocalOffset, vertexAxis);
srcProj += new Vector2(srcOffset, srcOffset);
// calculate intervals
// Calculate delta of min of one interval with max of the other.
// Note that if the intervals overlap then both deltas must be negative.
// I.e., delta>0 --> intervals don't overlap because one min greater than
// the other max.
float delta0 = srcProj.X - dstProj.Y;
float delta1 = dstProj.X - srcProj.Y;
// Are we seperated?
if (delta0 > 0.0f || delta1 > 0.0f)
{
// Yes, so test the dynamic intervals
// calculate speed along a particular axis
float speed = Vector2.Dot(refLocalVelocity, vertexAxis);
float epsilon = 0.0001f;
// ignore small speeds
if (Math.Abs(speed) < epsilon)
return false;
// The smallest absolute value must be the separation. The reason is that one
// delta measures separation between intervals, the other measures the span of
// the two intervals (we want separation, which is always smaller). Convert separation
// into time by dividing by the speed (adjust sign depending on whether we are going
// from source to dest or not).
timeAxis = delta0 * delta0 < delta1 * delta1 ? -delta0 / speed : delta1 / speed;
// successful collision?
return timeAxis >= 0.0f && timeAxis <= collisionTime;
}
else
{
// No, so we're overlapped.
//
// Let's get the interval of the smallest |delta0| and |delta1| and
// encode it as a negative value to signify an overlap rather than
// a disjoint collision in forward-time.
timeAxis = (delta0 > delta1) ? delta0 : delta1;
// Successful Collision!
return true;
}
}
