private bool CheckShapeIntersection(Vector2 posA, float cosA, float sinA, float scaleA, CollisionShape shapeA,
Vector2 posB, float cosB, float sinB, float scaleB, CollisionShape shapeB)
{
CircleCollisionShape circleA = shapeA as CircleCollisionShape;
PolygonCollisionShape polygonA = shapeA as PolygonCollisionShape;
CircleCollisionShape circleB = shapeB as CircleCollisionShape;
PolygonCollisionShape polygonB = shapeB as PolygonCollisionShape;
if (circleA != null && circleB != null)
{
return(CheckCircleCircleIntersection(posA, circleA,
posB, circleB));
}
if (circleB != null && polygonA != null)
{
return(CheckPolygonCircleIntersection(posA, cosA, sinA, scaleA, polygonA,
posB, circleB));
}
if (circleA != null && polygonB != null)
{
return(CheckPolygonCircleIntersection(posB, cosB, sinB, scaleB, polygonB,
posA, circleA));
}
if (polygonA != null && polygonB != null)
{
return(CheckPolygonPolygonIntersection(posA, cosA, sinA, scaleA, polygonA,
posB, cosB, sinB, scaleB, polygonB));
}
return(false);
}
private RaycastHit Raycast(ImmutableList <Entity> raycastEntities, Vector2 origin, Vector2 direction)
{
Vector2 v1 = new Vector2(-direction.Y, direction.X);
RaycastHit hit = new RaycastHit
{
LengthSquared = double.PositiveInfinity
};
// Only raycasts against polygons (not circles)
foreach (Entity raycastEntity in raycastEntities)
{
bool performRayCast = true;
TransformComponent raycastTransformComp = raycastEntity.GetComponent <TransformComponent>();
CollisionComponent raycastCollisionComp = raycastEntity.GetComponent <CollisionComponent>();
if ((raycastCollisionComp.CollisionMask & Constants.Collision.COLLISION_GROUP_RAYCAST) == 0)
{
performRayCast = false;
}
// Make or find a raycast collision group
if (performRayCast)
{
float cos = (float)Math.Cos(raycastTransformComp.Rotation),
sin = (float)Math.Sin(raycastTransformComp.Rotation);
foreach (CollisionShape collisionShape in raycastCollisionComp.CollisionShapes)
{
PolygonCollisionShape polygonShape = collisionShape as PolygonCollisionShape;
if (polygonShape != null)
{
for (int i = 0; i < polygonShape.Vertices.Length; i++)
{
int j = (i + 1) % polygonShape.Vertices.Length;
Vector2 a = polygonShape.Vertices[i] + polygonShape.Offset;
a *= raycastTransformComp.Scale;
a = new Vector2(cos * a.X - sin * a.Y, sin * a.X + cos * a.Y);
a += raycastTransformComp.Position;
Vector2 b = polygonShape.Vertices[j] + polygonShape.Offset;
b *= raycastTransformComp.Scale;
b = new Vector2(cos * b.X - sin * b.Y, sin * b.X + cos * b.Y);
b += raycastTransformComp.Position;
Vector2 v2 = a - b;
Vector2 v3 = a - origin;
Vector2 v4 = new Vector2(a.Y - b.Y, b.X - a.X);
double det = 1 / Vector2.Dot(v1, v2);
double t1 = det * Vector2.Dot(v3, v4);
double t2 = det * Vector2.Dot(v1, v3);
if (t2 >= 0 && t2 <= 1 &&
t1 >= 0)
{
// Hit
Vector2 newHit = (b - a) * (float)t2 + a;
float newLengthSquared = (newHit - origin).LengthSquared();
if (newLengthSquared < hit.LengthSquared)
{
hit.Position = newHit;
hit.Normal = new Vector2(-v2.Y, v2.X);
if (Vector2.Dot(direction, hit.Normal) > 0)
{
hit.Normal *= -1;
}
hit.Normal.Normalize();
hit.LengthSquared = newLengthSquared;
hit.Other = raycastEntity;
}
}
}
}
}
}
}
return(hit);
}
private bool CheckPolygonPolygonIntersection(Vector2 posA, float cosA, float sinA, float scaleA, PolygonCollisionShape polygonA,
Vector2 posB, float cosB, float sinB, float scaleB, PolygonCollisionShape polygonB)
{
Vector2[] worldPolyA = new Vector2[polygonA.Vertices.Length];
for (int i = 0; i < polygonA.Vertices.Length; i++)
{
worldPolyA[i] = new Vector2(cosA * polygonA.Vertices[i].X - sinA * polygonA.Vertices[i].Y,
sinA * polygonA.Vertices[i].X + cosA * polygonA.Vertices[i].Y) * scaleA + posA;
}
Vector2[] worldPolyB = new Vector2[polygonB.Vertices.Length];
for (int i = 0; i < polygonB.Vertices.Length; i++)
{
worldPolyB[i] = new Vector2(cosB * polygonB.Vertices[i].X - sinB * polygonB.Vertices[i].Y,
sinB * polygonB.Vertices[i].X + cosB * polygonB.Vertices[i].Y) * scaleB + posB;
}
for (int i = 0; i < worldPolyA.Length; i++)
{
int j = (i + 1) % worldPolyA.Length;
Vector2 edge = worldPolyA[j] - worldPolyA[i];
Vector2 axis = new Vector2(-edge.Y, edge.X);
axis.Normalize();
if (!PassSAT(worldPolyA, worldPolyB, axis))
{
// SAT: If any check does _not_ pass, they are _not_ intersecting
return(false);
}
}
for (int i = 0; i < worldPolyB.Length; i++)
{
int j = (i + 1) % worldPolyB.Length;
Vector2 edge = worldPolyB[j] - worldPolyB[i];
Vector2 axis = new Vector2(-edge.Y, edge.X);
axis.Normalize();
if (!PassSAT(worldPolyA, worldPolyB, axis))
{
// SAT: If any check does _not_ pass, they are _not_ intersecting
return(false);
}
}
return(true);
}
private bool CheckPolygonCircleIntersection(Vector2 posA, float cosA, float sinA, float scaleA, PolygonCollisionShape polygonA,
Vector2 posB, CircleCollisionShape circleB)
{
Vector2[] worldPolyA = new Vector2[polygonA.Vertices.Length];
for (int i = 0; i < polygonA.Vertices.Length; i++)
{
worldPolyA[i] = new Vector2(cosA * polygonA.Vertices[i].X - sinA * polygonA.Vertices[i].Y,
sinA * polygonA.Vertices[i].X + cosA * polygonA.Vertices[i].Y) * scaleA + posA;
}
BoundingCircle worldCircleB = new BoundingCircle(posB, circleB.Radius);
for (int i = 0; i < worldPolyA.Length; i++)
{
int j = (i + 1) % worldPolyA.Length;
Vector2 v1 = worldPolyA[j];
Vector2 v2 = worldPolyA[i];
// Check vertices; guarenteed to intersect if vertices
// are contained and less expensive than a segment intersection
// test.
if (worldCircleB.Contains(v1) || worldCircleB.Contains(v2))
{
return(true);
}
// Cast circle position onto segment
Vector2 segment = v2 - v1;
float circleOnSegment = Vector2.Dot(segment, worldCircleB.Position);
// Make sure segment is along the segment
if (circleOnSegment < 0)
{
continue;
}
if (circleOnSegment * circleOnSegment > segment.LengthSquared())
{
continue;
}
// Find point along segment
segment.Normalize();
Vector2 intersectionPoint = segment * circleOnSegment + v1;
// Check for intersection of intersection point
if (worldCircleB.Contains(intersectionPoint))
{
return(true);
}
}
return(false);
}
public void Draw(Matrix transformMatrix, float dt)
{
SpriteBatch.Begin(SpriteSortMode.Deferred,
BlendState.Opaque,
SamplerState.PointClamp,
null,
null,
null,
transformMatrix);
foreach (Entity entity in _collisionEntities)
{
CollisionComponent collisionComp = entity.GetComponent <CollisionComponent>();
TransformComponent transformComp = entity.GetComponent <TransformComponent>();
Vector2 position = transformComp.Position * FlipY;
float cos = (float)Math.Cos(-transformComp.Rotation);
float sin = (float)Math.Sin(-transformComp.Rotation);
float scale = transformComp.Scale;
foreach (CollisionShape shape in collisionComp.CollisionShapes)
{
Vector2 rotatedOffset = rotateAroundOrigin(shape.Offset, cos, sin);
BoundingRect AABB = shape.GetAABB(cos, sin, scale);
AABB.Min += position;
AABB.Max += position;
SpriteBatch.DrawRectangle(new Rectangle((int)AABB.Left,
(int)AABB.Bottom,
(int)AABB.Width,
(int)AABB.Height),
Color.BlueViolet,
1);
CircleCollisionShape circleCollisionShape = shape as CircleCollisionShape;
if (circleCollisionShape != null)
{
SpriteBatch.DrawCircle(position + rotatedOffset * scale,
circleCollisionShape.Radius * scale,
25,
Color.YellowGreen);
}
PolygonCollisionShape polygonCollisionShape = shape as PolygonCollisionShape;
if (polygonCollisionShape != null)
{
for (int i = 0; i < polygonCollisionShape.Vertices.Length; i++)
{
Vector2 v1 = polygonCollisionShape.Vertices[i] * FlipY;
Vector2 v2 = polygonCollisionShape.Vertices[(i + 1) % polygonCollisionShape.Vertices.Length] * FlipY;
SpriteBatch.DrawLine(position + (rotatedOffset + rotateAroundOrigin(v1, cos, sin)) * scale,
position + (rotatedOffset + rotateAroundOrigin(v2, cos, sin)) * scale,
Color.YellowGreen);
}
}
}
}
SpriteBatch.End();
}
