internal static bool CollidePolygonAndPoint(Vector2[] one, Vector2[] oneNormals, Transform oneTransform, Vector2 another, Transform anotherTransform, out CollisionResult result) { Vector2 worldPoint = anotherTransform.Mul(another); Vector2 anotherPoint = oneTransform.MulT(worldPoint); int index = -1; float dist = float.MaxValue; // Loop through all the edges for (int i = 0; i < oneNormals.Length; i++) { Vector2 normal = oneNormals[i]; float pointA = Vector2.Dot(anotherPoint, normal); // Find the projection of the polygon on the current axis float minA = 0; float maxA = 0; ProjectPolygon(normal, one, out minA, out maxA); if (pointA < minA || pointA > maxA) { result = default(CollisionResult); return(false); } float distance = Vector2.Dot(one[i] - anotherPoint, normal); if (distance < dist) { dist = distance; index = i; } } result = new CollisionResult(worldPoint, oneTransform.MulR(oneNormals[index]) * dist); return(true); }
//This part knows nothing about shapes and works only with raw data internal static bool CollidePoints(Vector2 one, Transform oneTransform, Vector2 another, Transform anotherTransform, out CollisionResult result) { Vector2 onePoint = oneTransform.Mul(one); Vector2 anotherPoint = anotherTransform.Mul(another); Vector2 dir = onePoint - anotherPoint; float distanceSqrd = dir.LengthSquared(); if (distanceSqrd > float.Epsilon) { result = default(CollisionResult); return(false); } #if !USE_MATHF dir /= (float)Math.Sqrt(distanceSqrd); // that function requires define and additional check #else dir *= Math.InvSqrt(distanceSqrd); #endif result = new CollisionResult(onePoint, dir); return(true); }
internal static bool CollideRectangleAndPoint(Vector2 anotherCenter, float anotherWidth, float anotherHeight, Transform anotherTransform, Vector2 one, Transform oneTransform, out CollisionResult result) { Vector2 worldPoint = oneTransform.Mul(one); Vector2 anotherPoint = (anotherTransform.MulT(worldPoint) - anotherCenter) * 2; // note the * 2 here, that is done to use width/height and not width*0.5 if (anotherPoint.X < -anotherWidth || anotherPoint.X > anotherWidth || anotherPoint.Y < -anotherHeight || anotherPoint.Y > anotherHeight) { result = default(CollisionResult); return(false); } float[] distances = { Math.Abs(-anotherWidth - worldPoint.X), // left Math.Abs(anotherHeight - worldPoint.Y), // top Math.Abs(anotherWidth - worldPoint.X), // right Math.Abs(-anotherHeight - worldPoint.Y), // bottom }; int index = -1; float dist = float.MaxValue; for (int i = 0; i < distances.Length; i++) { if (distances[i] < dist) { dist = distances[i]; index = i; } } Vector2 normal = anotherTransform.MulR(RectangleShape.RectangleNormals[index]) * dist; result = new CollisionResult(worldPoint, normal); return(true); }
public bool IntersectsWith(Transform ownTransform, IShape shape, Transform shapeTransform, out CollisionResult result) { return(CollisionHelper.CheckIntersection(this, ownTransform, shape, shapeTransform, out result)); }
// This functions take any pair of shapes and performs raw operations public static bool CheckIntersection(IShape one, Transform oneTransform, IShape another, Transform anotherTransform, out CollisionResult result) { // here goes the trick: we need to check most complex case first, so swap the shapes if ((int)one.ShapeType < (int)another.ShapeType) { bool cresult = CheckIntersection(another, anotherTransform, one, oneTransform, out result); if (cresult) { result.Invert(); } return(cresult); } switch (one.ShapeType) { // top level shape needs to be checked against all other kinds of shapes case ShapeType.Complex: { List <CollisionResult> collisionList = new List <CollisionResult>(((ComplexShape)one).Shapes.Length); int count = 0; foreach (IShape ownShape in ((ComplexShape)one).Shapes) { if (CheckIntersection(ownShape, oneTransform, another, anotherTransform, out result)) { collisionList.Add(result); count += result.Points.Length; } } if (count > 0) { List <Vector2> collisionPoints = new List <Vector2>(count); List <Vector2> collisionNormals = new List <Vector2>(count); foreach (CollisionResult collision in collisionList) { collisionPoints.AddRange(collision.Points); collisionNormals.AddRange(collision.Normals); } result = new CollisionResult(collisionPoints.ToArray(), collisionNormals.ToArray()); return(true); } } break; // second level don't need to be checked against top level, so minus one check here case ShapeType.Polygon: switch (another.ShapeType) { case ShapeType.Point: return(CollidePolygonAndPoint(((PolygonShape)one).Points, ((PolygonShape)one).Normals, oneTransform, ((PointShape)another).Position, anotherTransform, out result)); case ShapeType.Circle: return(CollidePolygonAndCircle(((PolygonShape)one).Points, ((PolygonShape)one).Normals, oneTransform, ((CircleShape)another).Center, ((CircleShape)another).Radius, anotherTransform, out result)); case ShapeType.Rectangle: return(CollidePolygons(((PolygonShape)one).Points, ((PolygonShape)one).Normals, oneTransform, ((RectangleShape)another).Points, ((RectangleShape)another).Normals, anotherTransform, out result)); case ShapeType.Polygon: return(CollidePolygons(((PolygonShape)one).Points, ((PolygonShape)one).Normals, oneTransform, ((PolygonShape)another).Points, ((PolygonShape)another).Normals, anotherTransform, out result)); } break; // minus one check here case ShapeType.Rectangle: switch (another.ShapeType) { case ShapeType.Point: return(CollideRectangleAndPoint(((RectangleShape)one).Center, ((RectangleShape)one).Width, ((RectangleShape)one).Height, oneTransform, ((PointShape)another).Position, anotherTransform, out result)); case ShapeType.Circle: // rectangle can be represented as polygon with the same result, but pure rect-circle calculation is faster #if RECT_AS_POLY return(CollidePolygonAndCircle(((RectangleShape)one).Points, ((RectangleShape)one).Normals, oneTransform, ((CircleShape)another).Center, ((CircleShape)another).Radius, anotherTransform, out result)); #else return(CollideRectangleAndCircle(((RectangleShape)one).Center, ((RectangleShape)one).Width, ((RectangleShape)one).Height, oneTransform, ((CircleShape)another).Center, ((CircleShape)another).Radius, anotherTransform, out result)); #endif case ShapeType.Rectangle: return(CollidePolygons(((RectangleShape)one).Points, ((RectangleShape)one).Normals, oneTransform, ((RectangleShape)another).Points, ((RectangleShape)another).Normals, anotherTransform, out result)); } break; // minus one check here case ShapeType.Circle: switch (another.ShapeType) { case ShapeType.Point: return(CollideCircleAndPoint(((CircleShape)one).Center, ((CircleShape)one).Radius, oneTransform, ((PointShape)another).Position, anotherTransform, out result)); case ShapeType.Circle: return(CollideCircles(((CircleShape)one).Center, ((CircleShape)one).Radius, oneTransform, ((CircleShape)another).Center, ((CircleShape)another).Radius, anotherTransform, out result)); } break; // Only one check left here - all other cases will be guaranteed checked above. I.e. Point/Polygon will become Polygon/Point case ShapeType.Point: { if (another.ShapeType == ShapeType.Point) { return(CollidePoints(((PointShape)one).Position, oneTransform, ((PointShape)another).Position, anotherTransform, out result)); } } break; } result = default(CollisionResult); return(false); }
internal static bool CollideCircles(Vector2 oneCenter, float oneRadius, Transform oneTransform, Vector2 anotherCenter, float anotherRadius, Transform anotherTransform, out CollisionResult result) { Vector2 onePoint = oneTransform.Mul(oneCenter); Vector2 anotherPoint = anotherTransform.Mul(anotherCenter); float radius = oneRadius + anotherRadius; Vector2 dir = anotherPoint - onePoint; float distanceSqrd = dir.LengthSquared(); if (distanceSqrd > radius * radius) { result = default(CollisionResult); return(false); } float distance = (float)Math.Sqrt(distanceSqrd); dir /= distance; float collisionLength = (oneRadius + anotherRadius - distance); Vector2 point = onePoint + dir * (oneRadius + collisionLength / 2); // center of the collision result = new CollisionResult(point, dir * collisionLength); // we need to shift one of circles to collision length distance return(true); }
internal static bool CollidePolygons(Vector2[] one, Vector2[] oneNormals, Transform oneTransform, Vector2[] nanotherPoints, Vector2[] nanotherNormals, Transform anotherTransform, out CollisionResult result) { result = default(CollisionResult); if (s_cachePoints == null || nanotherPoints.Length != s_cachePoints.Length) { s_cachePoints = new Vector2[nanotherPoints.Length]; s_cacheNormals = new Vector2[nanotherNormals.Length]; } Vector2[] anotherPoints = s_cachePoints; Vector2[] anotherNormals = s_cacheNormals; for (int i = 0; i < anotherPoints.Length; i++) { anotherPoints[i] = oneTransform.MulT(anotherTransform.Mul(nanotherPoints[i])); anotherNormals[i] = oneTransform.MulTR(anotherTransform.MulR(nanotherNormals[i])); } float minDistance = float.MaxValue; Vector2 minNormal = Vector2.Zero; // Loop through all the edges of both polygons for (int i = 0; i < oneNormals.Length; i++) { Vector2 normal = oneNormals[i]; // Find the projection of the polygon on the current axis float minA; float maxA; ProjectPolygon(normal, one, out minA, out maxA); float minB; float maxB; ProjectPolygon(normal, anotherPoints, out minB, out maxB); if (minA > maxB || maxA < minB) { return(false); } float distance = Math.Max(minA, minB) - Math.Min(maxA, maxB); if (distance < minDistance) { minDistance = distance; minNormal = normal; } } for (int i = 0; i < anotherNormals.Length; i++) { Vector2 normal = anotherNormals[i]; // Find the projection of the polygon on the current axis float minA; float maxA; ProjectPolygon(normal, one, out minA, out maxA); float minB; float maxB; ProjectPolygon(normal, anotherPoints, out minB, out maxB); if (minA > maxB || maxA < minB) { return(false); } float distance = Math.Max(minA, minB) - Math.Min(maxA, maxB); if (distance < minDistance) { minDistance = distance; minNormal = normal; } } float x; { Vector2 axisX = new Vector2(1, 0); float minA; float maxA; ProjectPolygon(axisX, one, out minA, out maxA); float minB; float maxB; ProjectPolygon(axisX, anotherPoints, out minB, out maxB); x = (Math.Max(minA, minB) + Math.Min(maxA, maxB)) / 2; } float y; { Vector2 axisY = new Vector2(0, 1); float minA; float maxA; ProjectPolygon(axisY, one, out minA, out maxA); float minB; float maxB; ProjectPolygon(axisY, anotherPoints, out minB, out maxB); y = (Math.Max(minA, minB) + Math.Min(maxA, maxB)) / 2; } // TODO: correct polygon-polygon collision result result = new CollisionResult(oneTransform.Mul(new Vector2(x, y)), oneTransform.MulR(minNormal)); return(true); }
internal static bool CollideCircleAndPoint(Vector2 anotherCenter, float anotherRadius, Transform anotherTransform, Vector2 one, Transform oneTransform, out CollisionResult result) { Vector2 onePoint = oneTransform.Mul(one); Vector2 anotherPoint = anotherTransform.Mul(anotherCenter); Vector2 dir = anotherPoint - onePoint; float distanceSqrd = dir.LengthSquared(); if (distanceSqrd > anotherRadius * anotherRadius) { result = default(CollisionResult); return(false); } result = new CollisionResult(onePoint, dir); // we need to shift the point to the full collision distance return(true); }
internal static bool CollidePolygonAndCircle(Vector2[] one, Vector2[] oneNormals, Transform oneTransform, Vector2 anotherCenter, float anotherRadius, Transform anotherTransform, out CollisionResult result) { result = default(CollisionResult); Vector2 worldCenter = anotherTransform.Mul(anotherCenter); Vector2 anotherPoint = oneTransform.MulT(worldCenter); #if OLD int index = -1; float dist = float.MaxValue; // Loop through all the edges for (int i = 0; i < oneNormals.Length; i++) { Vector2 normal = oneNormals[i]; float pointA = Vector2.Dot(anotherPoint, normal); // Find the projection of the polygon on the current axis float minA = 0; float maxA = 0; ProjectPolygon(normal, one, out minA, out maxA); if (pointA + anotherRadius < minA || pointA - anotherRadius > maxA) { return(false); } float distance = Vector2.Dot(one[i] - anotherPoint, normal); if (-distance > anotherRadius) { return(false); } if (distance < dist) { dist = distance; index = i; } } float x; { Vector2 axisX = new Vector2(1, 0); float minA = 0; float maxA = 0; ProjectPolygon(axisX, one, out minA, out maxA); float pointA = Vector2.Dot(anotherPoint, axisX); float minB = pointA - anotherRadius; float maxB = pointA + anotherRadius; x = (Math.Max(minA, minB) + Math.Min(maxA, maxB)) / 2; } float y; { Vector2 axisY = new Vector2(0, 1); float minA = 0; float maxA = 0; ProjectPolygon(axisY, one, out minA, out maxA); float pointA = Vector2.Dot(anotherPoint, axisY); float minB = pointA - anotherRadius; float maxB = pointA + anotherRadius; y = (Math.Max(minA, minB) + Math.Min(maxA, maxB)) / 2; } Vector2 resultPoint = oneTransform.Mul(new Vector2(x, y)); Vector2 resultNormal = oneTransform.MulR(oneNormals[index]); result = new CollisionResult(resultPoint, resultNormal); return(true); #elif true int index = -1; float dist = float.MaxValue; // Loop through all the edges for (int i = 0; i < oneNormals.Length; i++) { Vector2 normal = oneNormals[i]; float pointA = Vector2.Dot(anotherPoint, normal); // Find the projection of the polygon on the current axis float minA = 0; float maxA = 0; ProjectPolygon(normal, one, out minA, out maxA); if (pointA + anotherRadius < minA || pointA - anotherRadius > maxA) { return(false); } float distance = Vector2.Dot(one[i] - anotherPoint, normal); if (-distance > anotherRadius) { return(false); } if (distance < dist) { dist = distance; index = i; } } // Vertices that subtend the incident face. int vertIndex1 = index; int vertIndex2 = (vertIndex1 + 1) % one.Length; Vector2 v1 = one[vertIndex1]; Vector2 v2 = one[vertIndex2]; // If the center is inside the polygon ... if (-dist < float.Epsilon) { Vector2 localNormal = oneNormals[index]; Vector2 localPoint = (v1 + v2) / 2; Vector2 resultPoint = oneTransform.Mul(localPoint); Vector2 resultNormal = oneTransform.MulR(localNormal); result = new CollisionResult(resultPoint, resultNormal); return(true); } // Compute barycentric coordinates float u1 = Vector2.Dot(anotherPoint - v1, v2 - v1); float u2 = Vector2.Dot(anotherPoint - v2, v1 - v2); if (u1 <= 0.0f) { float r = Vector2.DistanceSquared(anotherPoint, v1); if (r > anotherRadius * anotherRadius) { return(false); } Vector2 localNormal = (anotherPoint - v1) / r; Vector2 resultPoint = oneTransform.Mul(v1); Vector2 resultNormal = oneTransform.MulR(localNormal);// * Math.Sqrt(r); result = new CollisionResult(resultPoint, resultNormal); } else if (u2 <= 0.0f) { float r = Vector2.DistanceSquared(anotherPoint, v2); if (r > anotherRadius * anotherRadius) { return(false); } Vector2 localNormal = (anotherPoint - v2) / r; Vector2 resultPoint = oneTransform.Mul(v2); Vector2 resultNormal = oneTransform.MulR(localNormal);// * Math.Sqrt(r); result = new CollisionResult(resultPoint, resultNormal); } else { Vector2 faceCenter = (v1 * u1 + v2 * u2) / (u1 + u2); //if (Vector2.DistanceSquared(anotherPoint, faceCenter) > anotherRadius * anotherRadius) //return false; float s = Vector2.Dot(oneNormals[vertIndex1], anotherPoint - faceCenter); if (s > anotherRadius) { return(false); } Vector2 localNormal = oneNormals[vertIndex1]; Vector2 resultPoint = oneTransform.Mul(faceCenter); Vector2 resultNormal = oneTransform.MulR(localNormal);// * s; result = new CollisionResult(resultPoint, resultNormal); } return(true); #else // Compute circle position in the frame of the polygon. // Find the min separating edge. int index = 0; float separation = float.MinValue; float radius = anotherRadius + Mathf.Epsilon; int vertexCount = one.Length; for (int i = 0; i < vertexCount; ++i) { float distance = Vector2.Dot(oneNormals[i], anotherPoint - one[i]); if (distance > radius) { //Early out. return(false); } if (distance > separation) { separation = distance; index = i; } } int vertIndex1 = index; int vertIndex2 = (vertIndex1 + 1) % one.Length; Vector2 v1 = one[vertIndex1]; Vector2 v2 = one[vertIndex2]; // If the center is inside the polygon ... if (separation < Mathf.Epsilon) { Vector2 localNormal = oneNormals[index]; Vector2 localPoint = (v1 + v2) / 2; Vector2 resultPoint = oneTransform.Mul(localPoint); Vector2 resultNormal = oneTransform.MulR(localNormal); result = new CollisionResult(resultPoint, resultNormal); return(true); } float u1 = Vector2.Dot(anotherPoint - v1, v2 - v1); float u2 = Vector2.Dot(anotherPoint - v2, v1 - v2); if (u1 <= 0.0f) { float r = Vector2.DistanceSquared(anotherPoint, v1); if (r > anotherRadius * anotherRadius) { return(false); } Vector2 localNormal = anotherPoint - v1; localNormal.Normalize(); Vector2 resultPoint = oneTransform.Mul(v1); Vector2 resultNormal = oneTransform.MulR(localNormal); result = new CollisionResult(resultPoint, resultNormal); } else if (u2 <= 0.0f) { float r = Vector2.DistanceSquared(anotherPoint, v2); if (r > anotherRadius * anotherRadius) { return(false); } Vector2 localNormal = anotherPoint - v2; localNormal.Normalize(); Vector2 resultPoint = oneTransform.Mul(v2); Vector2 resultNormal = oneTransform.MulR(localNormal); result = new CollisionResult(resultPoint, resultNormal); } else { Vector2 faceCenter = //(v1 + v2) / 2; (v1 * u1 + v2 * u2) / (u1 + u2); if (Vector2.DistanceSquared(anotherPoint, faceCenter) > anotherRadius * anotherRadius) { return(false); } float s = Vector2.Dot(oneNormals[vertIndex1], anotherPoint - faceCenter); if (s > anotherRadius) { return(false); } Vector2 localNormal = oneNormals[vertIndex1]; Vector2 resultPoint = oneTransform.Mul(faceCenter); Vector2 resultNormal = oneTransform.MulR(localNormal); result = new CollisionResult(resultPoint, resultNormal); } return(true); #endif }
internal static bool CollideRectangleAndCircle(Vector2 anotherCenter, float anotherWidth, float anotherHeight, Transform anotherTransform, Vector2 oneCenter, float oneRadius, Transform oneTransform, out CollisionResult result) { Vector2 anotherPoint = anotherTransform.MulT(oneTransform.Mul(oneCenter)) - anotherCenter; result = default(CollisionResult); // resulting X and Y are invalid in some rare cases, that why we need to rewrite this method or use RECT_AS_POLY float x; { Vector2 axisX = new Vector2(1, 0); float minA = -anotherWidth / 2; float maxA = anotherWidth / 2; float pointA = Vector2.Dot(anotherPoint, axisX); float minB = pointA - oneRadius; float maxB = pointA + oneRadius; if (minA > maxB || maxA < minB) { return(false); } x = (Math.Max(minA, minB) + Math.Min(maxA, maxB)) / 2; } float y; { Vector2 axisY = new Vector2(0, 1); float minA = -anotherHeight / 2; float maxA = anotherHeight / 2; float pointA = Vector2.Dot(anotherPoint, axisY); float minB = pointA - oneRadius; float maxB = pointA + oneRadius; if (minA > maxB || maxA < minB) { return(false); } y = (Math.Max(minA, minB) + Math.Min(maxA, maxB)) / 2; } if (Vector2.DistanceSquared(anotherPoint, new Vector2(x, y)) > oneRadius * oneRadius) { return(false); } float left = Math.Abs(-anotherWidth / 2 - x); // left float top = Math.Abs(anotherHeight / 2 - y); // top float right = Math.Abs(anotherWidth / 2 - x); // right float bottom = Math.Abs(-anotherHeight / 2 - y); // bottom int index = 0; float dist = left; if (top < dist) { dist = top; index = 1; } if (right < dist) { dist = right; index = 2; } if (bottom < dist) { dist = bottom; index = 3; } //for (int i = 0; i < distances.Length; i++) //{ // if (distances[i] < dist) // { // dist = distances[i]; // index = i; // } //} Vector2 normal = anotherTransform.MulR(RectangleShape.RectangleNormals[index]) * dist; result = new CollisionResult(anotherTransform.Mul(new Vector2(x, y) + anotherCenter), normal); return(true); }