private Point[] CalculateRectangleCorners(CameraSpacePoint pointA, CameraSpacePoint pointB, int widthA, int widthB)
{
// Calculate the vector that points from PointA to PointB.
Vector2 direction = new Vector2(pointB.X - pointA.X, pointB.Y - pointA.Y);
direction = Vector2.Normalize(direction);
// Calculate a perpendicular Vector to the direction.
Vector2 perpendicular = new Vector2(direction.Y, -direction.X);
float correctedWidthA = widthA * MATRIXUNIT_IN_METER;
float correctedWidthB = widthB * MATRIXUNIT_IN_METER;
CameraSpacePoint pointC = new CameraSpacePoint
{
X = pointA.X + perpendicular.X * (correctedWidthA / 2),
Y = pointA.Y + perpendicular.Y * (correctedWidthA / 2)
};
CameraSpacePoint pointD = new CameraSpacePoint
{
X = pointA.X - perpendicular.X * (correctedWidthA / 2),
Y = pointA.Y - perpendicular.Y * (correctedWidthA / 2)
};
CameraSpacePoint pointE = new CameraSpacePoint
{
X = pointB.X - perpendicular.X * (correctedWidthB / 2),
Y = pointB.Y - perpendicular.Y * (correctedWidthB / 2)
};
CameraSpacePoint pointF = new CameraSpacePoint
{
X = pointB.X + perpendicular.X * (correctedWidthB / 2),
Y = pointB.Y + perpendicular.Y * (correctedWidthB / 2)
};
Point[] points = new Point[]
{
pointC.ToGridPosition(),
pointD.ToGridPosition(),
pointE.ToGridPosition(),
pointF.ToGridPosition()
};
//points[0].CalculateCircle(widthA, circlePositions);
//points[1] = points[1].NearestPoint(circlePositions);
//points[2].CalculateCircle(widthB, circlePositions);
//points[3] = points[3].NearestPoint(circlePositions);
return(points);
}
private void DrawCircle(CameraSpacePoint pointA, int radius)
{
linePositions.Clear();
Point centerPoint = pointA.ToGridPosition();
centerPoint.CalculateCircle(radius, linePositions);
for (int y = centerPoint.Y - radius; y <= centerPoint.Y + radius; y++)
{
FindXValues(linePositions, xValues, y);
for (int x = xValues[0]; x <= xValues[1]; x++)
{
SetMatrixValue(x, y);
}
}
}