// Check if polygon A is going to collide with polygon B for the given velocity
public static PolygonCollisionResult PolygonCollision(Polygon polygonA, Polygon polygonB, Vector velocity)
{
PolygonCollisionResult result = new PolygonCollisionResult();
result.Intersect = true;
result.WillIntersect = true;
int edgeCountA = polygonA.Edges.Count;
int edgeCountB = polygonB.Edges.Count;
double minIntervalDistance = Double.PositiveInfinity;
Vector translationAxis = new Vector();
Vector edge;
// Loop through all the edges of both polygons
for (int edgeIndex = 0; edgeIndex < edgeCountA + edgeCountB; edgeIndex++)
{
if (edgeIndex < edgeCountA)
{
edge = polygonA.Edges[edgeIndex];
}
else
{
edge = polygonB.Edges[edgeIndex - edgeCountA];
}
// ===== 1. Find if the polygons are currently intersecting =====
// Find the axis perpendicular to the current edge
Vector axis = new Vector(-edge.Y, edge.X);
axis.Normalize();
// Find the projection of the polygon on the current axis
double minA = 0; double minB = 0; double maxA = 0; double maxB = 0;
ProjectPolygon(axis, polygonA, ref minA, ref maxA);
ProjectPolygon(axis, polygonB, ref minB, ref maxB);
// Check if the polygon projections are currentlty intersecting
if (IntervalDistance(minA, maxA, minB, maxB) > 0)
{
result.Intersect = false;
}
// ===== 2. Now find if the polygons *will* intersect =====
// Project the velocity on the current axis
double velocityProjection = axis.DotProduct(velocity);
// Get the projection of polygon A during the movement
if (velocityProjection < 0)
{
minA += velocityProjection;
}
else
{
maxA += velocityProjection;
}
// Do the same test as above for the new projection
double intervalDistance = IntervalDistance(minA, maxA, minB, maxB);
if (intervalDistance > 0)
{
result.WillIntersect = false;
}
// If the polygons are not intersecting and won't intersect, exit the loop
if (!result.Intersect && !result.WillIntersect)
{
break;
}
// Check if the current interval distance is the minimum one. If so store
// the interval distance and the current distance.
// This will be used to calculate the minimum translation vector
intervalDistance = Math.Abs(intervalDistance);
if (intervalDistance < minIntervalDistance)
{
minIntervalDistance = intervalDistance;
translationAxis = axis;
Vector d = polygonA.Center - polygonB.Center;
if (d.DotProduct(translationAxis) < 0)
{
translationAxis = -translationAxis;
}
}
}
// The minimum translation vector can be used to push the polygons appart.
// First moves the polygons by their velocity
// then move polygonA by MinimumTranslationVector.
if (result.WillIntersect)
{
result.MinimumTranslationVector = translationAxis * minIntervalDistance;
}
return(result);
}
// Check if polygon A is going to collide with polygon B for the given velocity
public static PolygonCollisionResult PolygonCollision(Polygon polygonA, Polygon polygonB, Vector velocity)
{
PolygonCollisionResult result = new PolygonCollisionResult();
result.Intersect = true;
result.WillIntersect = true;
int edgeCountA = polygonA.Edges.Count;
int edgeCountB = polygonB.Edges.Count;
double minIntervalDistance = Double.PositiveInfinity;
Vector translationAxis = new Vector();
Vector edge;
// Loop through all the edges of both polygons
for (int edgeIndex = 0; edgeIndex < edgeCountA + edgeCountB; edgeIndex++)
{
if (edgeIndex < edgeCountA)
{
edge = polygonA.Edges[edgeIndex];
}
else
{
edge = polygonB.Edges[edgeIndex - edgeCountA];
}
// ===== 1. Find if the polygons are currently intersecting =====
// Find the axis perpendicular to the current edge
Vector axis = new Vector(-edge.Y, edge.X);
axis.Normalize();
// Find the projection of the polygon on the current axis
double minA = 0; double minB = 0; double maxA = 0; double maxB = 0;
ProjectPolygon(axis, polygonA, ref minA, ref maxA);
ProjectPolygon(axis, polygonB, ref minB, ref maxB);
// Check if the polygon projections are currentlty intersecting
if (IntervalDistance(minA, maxA, minB, maxB) > 0) result.Intersect = false;
// ===== 2. Now find if the polygons *will* intersect =====
// Project the velocity on the current axis
double velocityProjection = axis.DotProduct(velocity);
// Get the projection of polygon A during the movement
if (velocityProjection < 0)
{
minA += velocityProjection;
}
else
{
maxA += velocityProjection;
}
// Do the same test as above for the new projection
double intervalDistance = IntervalDistance(minA, maxA, minB, maxB);
if (intervalDistance > 0) result.WillIntersect = false;
// If the polygons are not intersecting and won't intersect, exit the loop
if (!result.Intersect && !result.WillIntersect) break;
// Check if the current interval distance is the minimum one. If so store
// the interval distance and the current distance.
// This will be used to calculate the minimum translation vector
intervalDistance = Math.Abs(intervalDistance);
if (intervalDistance < minIntervalDistance)
{
minIntervalDistance = intervalDistance;
translationAxis = axis;
Vector d = polygonA.Center - polygonB.Center;
if (d.DotProduct(translationAxis) < 0) translationAxis = -translationAxis;
}
}
// The minimum translation vector can be used to push the polygons appart.
// First moves the polygons by their velocity
// then move polygonA by MinimumTranslationVector.
if (result.WillIntersect) result.MinimumTranslationVector = translationAxis * minIntervalDistance;
return result;
}
// Calculate the projection of a polygon on an axis and returns it as a [min, max] interval
public static void ProjectPolygon(Vector axis, Polygon polygon, ref double min, ref double max)
{
// To project a point on an axis use the dot product
double d = axis.DotProduct(polygon.Points[0]);
min = d;
max = d;
for (int i = 0; i < polygon.Points.Count; i++)
{
d = polygon.Points[i].DotProduct(axis);
if (d < min)
{
min = d;
}
else
{
if (d > max)
{
max = d;
}
}
}
}
/// <summary>
/// Find rectangles in image and add possible rectangle candidates as temporary but known objects or updates
/// existing objects from previous frames.
/// </summary>
/// <param name="occlusionTracking"></param>
/// <param name="grayImage"></param>
/// <param name="outputImage"></param>
/// <param name="updateTime"></param>
/// <param name="objects"></param>
private RectangularObject[] FindRectangles(bool occlusionTracking, Image<Gray, byte> grayImage, ref Image<Rgb, byte> outputImage, DateTime updateTime, RectangularObject[] objects)
{
var newObjects = new List<RectangularObject>();
var imageWidth = grayImage.Width;
var imageHeight = grayImage.Height;
var pixels = imageWidth * imageHeight;
var diagonal = Math.Sqrt(Math.Pow(imageWidth, 2) + Math.Pow(imageHeight, 2));
var maxRestoreDistance = (MaxRestoreDistance / 100.0) * diagonal;
using (var storage = new MemStorage())
{
for (var contours = grayImage.FindContours(CHAIN_APPROX_METHOD.CV_CHAIN_APPROX_SIMPLE, IsRetrieveExternal ? RETR_TYPE.CV_RETR_EXTERNAL : RETR_TYPE.CV_RETR_LIST, storage); contours != null; contours = contours.HNext)
{
var lowApproxContour = contours.ApproxPoly(contours.Perimeter * 0.015, storage);
if (lowApproxContour.Area > ((MinContourArea / 100.0) * pixels) && lowApproxContour.Area < ((MaxContourArea / 100.0) * pixels)) //only consider contours with area greater than
{
if (IsRenderContent && IsDrawAllContours)
outputImage.Draw(lowApproxContour, Rgbs.BlueTorquoise, 1);
//outputImage.Draw(currentContour.GetConvexHull(ORIENTATION.CV_CLOCKWISE), Rgbs.BlueTorquoise, 2);
// Continue with next contour if current contour is not a rectangle.
List<DPoint> points;
if (!IsPlausibleRectangle(lowApproxContour, MinAngle, MaxAngle, MinDetectRightAngles, out points)) continue;
var highApproxContour = contours.ApproxPoly(contours.Perimeter * 0.05, storage);
if (IsRenderContent && IsDrawAllContours)
outputImage.Draw(highApproxContour, Rgbs.Yellow, 1);
var rectangle = highApproxContour.BoundingRectangle;
var minAreaRect = highApproxContour.GetMinAreaRect(storage);
var polygon = new Polygon(points.ToArray(), imageWidth, imageHeight);
var contourPoints = highApproxContour.ToArray();
if (!UpdateObject(occlusionTracking, maxRestoreDistance, rectangle, minAreaRect, polygon, contourPoints, updateTime, objects))
{
newObjects.Add(CreateObject(NextId(), rectangle, minAreaRect, polygon, contourPoints, updateTime));
}
}
}
}
return newObjects.ToArray();
}
/// <summary>
/// Updates an object if it finds an object from last frame at a max restore distance.
/// </summary>
/// <param name="occluded"></param>
/// <param name="maxRestoreDistance"></param>
/// <param name="boundingRectangle"></param>
/// <param name="minAreaRect"></param>
/// <param name="polygon"></param>
/// <param name="points"></param>
/// <param name="updateTime"></param>
/// <param name="objects"></param>
/// <returns></returns>
private static bool UpdateObject(bool occluded, double maxRestoreDistance, Rectangle boundingRectangle, MCvBox2D minAreaRect, Polygon polygon, DPoint[] points, DateTime updateTime, IEnumerable<RectangularObject> objects)
{
var cCenter = minAreaRect.center;
RectangularObject candidate = null;
var leastDistance = double.MaxValue;
foreach (var obj in objects)
{
var oCenter = obj.Shape.center;
var distance = Math.Sqrt(Math.Pow(oCenter.X - cCenter.X, 2) + Math.Pow(oCenter.Y - cCenter.Y, 2));
if (!(leastDistance > distance)) continue;
candidate = obj;
leastDistance = distance;
}
if (leastDistance > maxRestoreDistance || candidate == null)
return false;
candidate.State = occluded ? TrackingState.Occluded : TrackingState.Tracked;
candidate.LastUpdate = updateTime;
candidate.Center = new WPoint(cCenter.X, cCenter.Y);
candidate.Bounds = boundingRectangle;
candidate.Shape = minAreaRect;
candidate.Polygon = polygon;
candidate.Points = points;
return true;
}
/// <summary>
/// Adds a new temporary object that will be used to identify itself in the preceeding frames.
/// </summary>
/// <param name="id"></param>
/// <param name="boundingRectangle"></param>
/// <param name="minAreaRect"></param>
/// <param name="polygon"></param>
/// <param name="points"></param>
/// <param name="updateTime"></param>
private static RectangularObject CreateObject(long id, Rectangle boundingRectangle, MCvBox2D minAreaRect, Polygon polygon, DPoint[] points, DateTime updateTime)
{
return new RectangularObject
{
Id = id,
State = TrackingState.Tracked,
LastUpdate = updateTime,
Center = new WPoint(minAreaRect.center.X, minAreaRect.center.Y),
Bounds = boundingRectangle,
Shape = minAreaRect,
Polygon = polygon,
Points = points,
};
}
