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(); }