/// <summary>
/// builds the Polygon edge normals. These are lazily created and updated only by the edgeNormals getter
/// </summary>
void buildEdgeNormals()
{
// for boxes we only require 2 edges since the other 2 are parallel
var totalEdges = isBox ? 2 : points.Length;
if (_edgeNormals == null || _edgeNormals.Length != totalEdges)
{
_edgeNormals = new Vector2[totalEdges];
}
Vector2 p2;
for (var i = 0; i < totalEdges; i++)
{
var p1 = points[i];
if (i + 1 >= points.Length)
{
p2 = points[0];
}
else
{
p2 = points[i + 1];
}
var perp = Vector2Ext.perpendicular(ref p1, ref p2);
Vector2Ext.normalize(ref perp);
_edgeNormals[i] = perp;
}
return;
}
/// <summary>
/// iterates all the edges of the polygon and gets the closest point on any edge to point. Returns via out the squared distance
/// to the closest point and the normal of the edge it is on. point should be in the space of the Polygon (point - poly.position)
/// </summary>
/// <returns>The closest point on polygon to point.</returns>
/// <param name="point">Point.</param>
/// <param name="distanceSquared">Distance squared.</param>
/// <param name="edgeNormal">Edge normal.</param>
public static Vector2 getClosestPointOnPolygonToPoint(Vector2[] points, Vector2 point, out float distanceSquared, out Vector2 edgeNormal)
{
distanceSquared = float.MaxValue;
edgeNormal = Vector2.Zero;
var closestPoint = Vector2.Zero;
float tempDistanceSquared;
for (var i = 0; i < points.Length; i++)
{
var j = i + 1;
if (j == points.Length)
{
j = 0;
}
var closest = ShapeCollisions.closestPointOnLine(points[i], points[j], point);
Vector2.DistanceSquared(ref point, ref closest, out tempDistanceSquared);
if (tempDistanceSquared < distanceSquared)
{
distanceSquared = tempDistanceSquared;
closestPoint = closest;
// get the normal of the line
var line = points[j] - points[i];
edgeNormal.X = -line.Y;
edgeNormal.Y = line.X;
}
}
Vector2Ext.normalize(ref edgeNormal);
return(closestPoint);
}
static bool collideCircles(CircleShape circleA, ref FSTransform firstTransform, CircleShape circleB, ref FSTransform secondTransform, out FSCollisionResult result)
{
result = new FSCollisionResult();
Collision.collideCircles(ref _manifold, circleA, ref firstTransform, circleB, ref secondTransform);
if (_manifold.pointCount > 0)
{
// this is essentically directly from ContactSolver.WorldManifold.Initialize. To avoid doing the extra math twice we duplicate this code
// here because it doesnt return some values we need to calculate separation
var pointA = MathUtils.mul(ref firstTransform, _manifold.localPoint);
var pointB = MathUtils.mul(ref secondTransform, _manifold.points[0].localPoint);
result.normal = pointA - pointB;
Vector2Ext.normalize(ref result.normal);
var cA = pointA - circleA.radius * result.normal;
var cB = pointB + circleB.radius * result.normal;
result.point = 0.5f * (cA + cB);
result.point *= FSConvert.simToDisplay;
var separation = Vector2.Dot(pointA - pointB, result.normal) - circleA.radius - circleB.radius;
result.minimumTranslationVector = result.normal * Math.Abs(separation);
#if DEBUG_FSCOLLISIONS
Debug.drawPixel(result.point, 5, Color.Red, 0.2f);
Debug.drawLine(result.point, result.point + result.normal * 20, Color.Yellow, 0.2f);
#endif
return(true);
}
return(false);
}
static bool collideEdgeAndCircle(EdgeShape edge, ref FSTransform edgeTransform, CircleShape circle, ref FSTransform circleTransform, out FSCollisionResult result)
{
result = new FSCollisionResult();
Collision.collideEdgeAndCircle(ref _manifold, edge, ref edgeTransform, circle, ref circleTransform);
if (_manifold.pointCount > 0)
{
// code adapted from PositionSolverManifold.Initialize
if (_manifold.type == ManifoldType.Circles)
{
// this is essentically directly from ContactSolver.WorldManifold.Initialize. To avoid doing the extra math twice we duplicate this code
// here because it doesnt return some values we need to calculate separation
var pointA = MathUtils.mul(ref edgeTransform, _manifold.localPoint);
var pointB = MathUtils.mul(ref circleTransform, _manifold.points[0].localPoint);
result.normal = pointA - pointB;
Vector2Ext.normalize(ref result.normal);
var cA = pointA - edge.radius * result.normal;
var cB = pointB + circle.radius * result.normal;
result.point = 0.5f * (cA + cB);
result.point *= FSConvert.simToDisplay;
var separation = Vector2.Dot(pointA - pointB, result.normal) - edge.radius - circle.radius;
// Ensure normal points from A to B
Vector2.Negate(ref result.normal, out result.normal);
result.minimumTranslationVector = result.normal * Math.Abs(separation);
}
else // FaceA
{
result.normal = MathUtils.mul(edgeTransform.q, _manifold.localNormal);
var planePoint = MathUtils.mul(ref edgeTransform, _manifold.localPoint);
var clipPoint = MathUtils.mul(ref circleTransform, _manifold.points[0].localPoint);
var separation = Vector2.Dot(clipPoint - planePoint, result.normal) - edge.radius - circle.radius;
result.point = (clipPoint - result.normal * circle.radius) * FSConvert.simToDisplay;
result.minimumTranslationVector = result.normal * -separation;
}
#if DEBUG_FSCOLLISIONS
Debug.drawPixel(result.point, 5, Color.Red, 0.2f);
Debug.drawLine(result.point, result.point + result.normal * 20, Color.Yellow, 0.2f);
#endif
return(true);
}
return(false);
}
/// <summary>
/// works for circles whos center is in the box as well as just overlapping with the center out of the box.
/// </summary>
/// <returns><c>true</c>, if to box was circled, <c>false</c> otherwise.</returns>
/// <param name="circle">First.</param>
/// <param name="box">Second.</param>
/// <param name="result">Result.</param>
public static bool circleToBox(Circle circle, Box box, out CollisionResult result)
{
result = new CollisionResult();
var closestPointOnBounds =
box.bounds.getClosestPointOnRectangleBorderToPoint(circle.position, out result.normal);
// deal with circles whos center is in the box first since its cheaper to see if we are contained
if (box.containsPoint(circle.position))
{
result.point = closestPointOnBounds;
// calculate mtv. Find the safe, non-collided position and get the mtv from that.
var safePlace = closestPointOnBounds + result.normal * circle.radius;
result.minimumTranslationVector = circle.position - safePlace;
return(true);
}
float sqrDistance;
Vector2.DistanceSquared(ref closestPointOnBounds, ref circle.position, out sqrDistance);
// see if the point on the box is less than radius from the circle
if (sqrDistance == 0)
{
result.minimumTranslationVector = result.normal * circle.radius;
}
else if (sqrDistance <= circle.radius * circle.radius)
{
result.normal = circle.position - closestPointOnBounds;
var depth = result.normal.Length() - circle.radius;
result.point = closestPointOnBounds;
Vector2Ext.normalize(ref result.normal);
result.minimumTranslationVector = depth * result.normal;
return(true);
}
return(false);
}
