public int time2Point(AForge.DoublePoint point, double speed)
{
int distance = distance2Middle(point);
int time = (int)(distance / speed);
return(time);
}
public void RoundTest( double x, double y, int expectedX, int expectedY )
{
DoublePoint dPoint = new DoublePoint( x, y );
IntPoint iPoint = new IntPoint( expectedX, expectedY );
Assert.AreEqual( iPoint, dPoint.Round( ) );
}
/// <summary>
/// Calculate Euclidean distance between two points.
/// </summary>
///
/// <param name="anotherPoint">Point to calculate distance to.</param>
///
/// <returns>Returns Euclidean distance between this point and
/// <paramref name="anotherPoint"/> points.</returns>
///
public double DistanceTo( DoublePoint anotherPoint )
{
double dx = X - anotherPoint.X;
double dy = Y - anotherPoint.Y;
return System.Math.Sqrt( dx * dx + dy * dy );
}
/// <summary>
/// Initializes a new instance of the <see cref="TrapezoidalFunction"/> class.
///
/// With four points the shape is known as flat fuzzy number or fuzzy interval (/--\).
/// </summary>
///
/// <param name="m1">X value where the degree of membership starts to raise.</param>
/// <param name="m2">X value where the degree of membership reaches the maximum value.</param>
/// <param name="m3">X value where the degree of membership starts to fall.</param>
/// <param name="m4">X value where the degree of membership reaches the minimum value.</param>
/// <param name="max">The maximum value that the membership will reach, [0, 1].</param>
/// <param name="min">The minimum value that the membership will reach, [0, 1].</param>
///
public TrapezoidalFunction( double m1, double m2, double m3, double m4, double max, double min )
: this( 4 )
{
points[0] = new DoublePoint( m1, min );
points[1] = new DoublePoint( m2, max );
points[2] = new DoublePoint( m3, max );
points[3] = new DoublePoint( m4, min );
}
private readonly double k; // line's slope
#endregion Fields
#region Constructors
private Line( DoublePoint start, DoublePoint end )
{
if ( start == end )
{
throw new ArgumentException( "Start point of the line cannot be the same as its end point." );
}
k = ( end.Y - start.Y ) / ( end.X - start.X );
b = Double.IsInfinity( k ) ? start.X : start.Y - k * start.X;
}
public AForge.DoublePoint getNearestPoint(AForge.DoublePoint point)
{
DoublePoint closestPoint = points[0];
double distance2P, minDistance = closestPoint.DistanceTo(point);
foreach (DoublePoint p in points)
{
distance2P = point.DistanceTo(p);
if (distance2P < minDistance)
{
closestPoint = p;
minDistance = closestPoint.DistanceTo(point);
}
}
return(closestPoint);
}
public int distance2Middle(AForge.DoublePoint point)
{
return((int)middlePoint.DistanceTo(point));
}
public DoublePoint[] getHeadingEdges(AForge.DoublePoint robotPosition, AForge.DoublePoint robotHeading)
{
ControlStrategy.isAtHeading(this, robotPosition, robotHeading);
return(headingEdges);
}
/// <summary>
/// Subtraction operator - subtracts scalar from the specified point.
/// </summary>
///
/// <param name="point">Point to decrease coordinates of.</param>
/// <param name="valueToSubtract">Value to subtract from coordinates of the specified point.</param>
///
/// <returns>Returns new point which coordinates equal to coordinates of
/// the specified point decreased by specified value.</returns>
///
public static DoublePoint Subtract(DoublePoint point, double valueToSubtract)
{
return(new DoublePoint(point.X - valueToSubtract, point.Y - valueToSubtract));
}
/// <summary>
/// Subtraction operator - subtracts scalar from the specified point.
/// </summary>
///
/// <param name="point">Point to decrease coordinates of.</param>
/// <param name="valueToSubtract">Value to subtract from coordinates of the specified point.</param>
///
/// <returns>Returns new point which coordinates equal to coordinates of
/// the specified point decreased by specified value.</returns>
///
public static DoublePoint Subtract( DoublePoint point, double valueToSubtract )
{
return new DoublePoint( point.X - valueToSubtract, point.Y - valueToSubtract );
}
/// <summary>
/// Subtraction operator - subtracts values of two points.
/// </summary>
///
/// <param name="point1">Point to subtract from.</param>
/// <param name="point2">Point to subtract.</param>
///
/// <returns>Returns new point which coordinates equal to difference of corresponding
/// coordinates of specified points.</returns>
///
public static DoublePoint Subtract( DoublePoint point1, DoublePoint point2 )
{
return new DoublePoint( point1.X - point2.X, point1.Y - point2.Y );
}
/*
* This function firstly calculates the closest point, drives towards it and finally starts the path.
*/
public override void calculateNextMove(AForge.DoublePoint robotPosition, double speed, AForge.DoublePoint heading, List <Robot> neighbors, out double referenceSpeed, out AForge.DoublePoint referenceHeading)
{
if (!onLine)
{
pointCount = 0;
double distance = robotPosition.DistanceTo(points[pointCount]);
for (int i = 0; i < points.Count(); i++)
{
if (distance > points[i].DistanceTo(robotPosition))
{
pointCount = i;
distance = points[pointCount].DistanceTo(robotPosition);
}
}
if (distance < closeUpLimit)
{
onLine = true;
}
else
{
goToPoint.calculateNextMove(points[pointCount], robotPosition, speed, heading, neighbors, out referenceSpeed, out referenceHeading);
return;
}
}
if (onLine)
{
double distance = robotPosition.DistanceTo(points[pointCount]);
if (distance <= closeUpLimit)
{
if (direction == 1)
{
pointCount = (pointCount >= points.Count() - 1 ? 0 : pointCount + direction);
}
else
{
pointCount = (pointCount >= 1 ? points.Count() - 1 : pointCount + direction);
//pointCount = (pointCount > 1 ? pointCount - 1 : points.Count() - 1); //Shouldn't it be this?
}
}
else
{
onLine = false;
}
goToPoint.calculateNextMove(points[pointCount], robotPosition, speed, heading, neighbors, out referenceSpeed, out referenceHeading);
if (fifoStrategy != null)
{
fifoStrategy.calculateNextMove(robotPosition, referenceSpeed, referenceHeading, neighbors, out referenceSpeed, out referenceHeading);
}
return;
}
//referenceSpeed = speed; //uncomment if using m3pi robots
//$$$$$Changes/Additions made for RC cars$$$$$//
referenceSpeed = Program.testSpeed;
//$$$$$$$$$$//
referenceHeading = heading;
}
/// <summary>
/// Initializes a new instance of the <see cref="TrapezoidalFunction"/> class.
///
/// With two points and an edge this shape can be a left fuzzy number (/) or a right fuzzy number (\).
/// </summary>
///
/// <param name="m1">Edge = Left: X value where the degree of membership starts to raise.
/// Edge = Right: X value where the function starts, with maximum degree of membership. </param>
/// <param name="m2">Edge = Left: X value where the degree of membership reaches the maximum.
/// Edge = Right: X value where the degree of membership reaches minimum value. </param>
/// <param name="max">The maximum value that the membership will reach, [0, 1].</param>
/// <param name="min">The minimum value that the membership will reach, [0, 1].</param>
/// <param name="edge">Trapezoid's <see cref="EdgeType"/>.</param>
///
public TrapezoidalFunction( double m1, double m2, double max, double min, EdgeType edge )
: this( 2 )
{
if ( edge == EdgeType.Left )
{
points[0] = new DoublePoint( m1, min );
points[1] = new DoublePoint( m2, max );
}
else
{
points[0] = new DoublePoint( m1, max );
points[1] = new DoublePoint( m2, min );
}
}
/// <summary>
/// Constructs a <see cref="Line"/> from a point and an angle (in degrees).
/// </summary>
///
/// <param name="point">The minimum distance from the line to the origin.</param>
/// <param name="theta">The angle of the normal vector from the origin to the line.</param>
///
/// <remarks><para><paramref name="theta"/> is the counterclockwise rotation from
/// the positive X axis to the vector through the origin and normal to the line.</para>
/// <para>This means that if <paramref name="theta"/> is in [0,180), the point on the line
/// closest to the origin is on the positive X or Y axes, or in quadrants I or II. Likewise,
/// if <paramref name="theta"/> is in [180,360), the point on the line closest to the
/// origin is on the negative X or Y axes, or in quadrants III or IV.</para></remarks>
///
/// <returns>Returns a <see cref="Line"/> representing the specified line.</returns>
///
public static Line FromPointTheta( DoublePoint point, double theta )
{
return new Line( point, theta );
}
/// <summary>
/// Creates a <see cref="Line"/> that goes through the two specified points.
/// </summary>
///
/// <param name="point1">One point on the line.</param>
/// <param name="point2">Another point on the line.</param>
///
/// <returns>Returns a <see cref="Line"/> representing the line between <paramref name="point1"/>
/// and <paramref name="point2"/>.</returns>
///
/// <exception cref="ArgumentException">Thrown if the two points are the same.</exception>
///
public static Line FromPoints( DoublePoint point1, DoublePoint point2 )
{
return new Line( point1, point2 );
}
private Line( DoublePoint point, double theta )
{
theta *= Math.PI / 180;
k = -1 / Math.Tan( theta );
if ( !Double.IsInfinity( k ) )
{
b = point.Y - k * point.X;
}
else
{
b = point.X;
}
}
/// <summary>
/// Calculate Euclidean distance between two points.
/// </summary>
///
/// <param name="anotherPoint">Point to calculate distance to.</param>
///
/// <returns>Returns Euclidean distance between this point and
/// <paramref name="anotherPoint"/> points.</returns>
///
public double DistanceTo( DoublePoint anotherPoint )
{
double dx = X - anotherPoint.X;
double dy = Y - anotherPoint.Y;
return Mathf.Sqrt( (float) (dx * dx + dy * dy) );
}
/// <summary>
/// Division operator - divides coordinates of the specified point by scalar value.
/// </summary>
///
/// <param name="point">Point to divide coordinates of.</param>
/// <param name="factor">Division factor.</param>
///
/// <returns>Returns new point which coordinates equal to coordinates of
/// the specified point divided by specified value.</returns>
///
public static DoublePoint Divide(DoublePoint point, double factor)
{
return(new DoublePoint(point.X / factor, point.Y / factor));
}
/// <summary>
/// Multiplication operator - multiplies coordinates of the specified point by scalar value.
/// </summary>
///
/// <param name="point">Point to multiply coordinates of.</param>
/// <param name="factor">Multiplication factor.</param>
///
/// <returns>Returns new point which coordinates equal to coordinates of
/// the specified point multiplied by specified value.</returns>
///
public static DoublePoint Multiply(DoublePoint point, double factor)
{
return(new DoublePoint(point.X * factor, point.Y * factor));
}
/// <summary>
/// Addition operator - adds values of two points.
/// </summary>
///
/// <param name="point1">First point for addition.</param>
/// <param name="point2">Second point for addition.</param>
///
/// <returns>Returns new point which coordinates equal to sum of corresponding
/// coordinates of specified points.</returns>
///
public static DoublePoint Add(DoublePoint point1, DoublePoint point2)
{
return(new DoublePoint(point1.X + point2.X, point1.Y + point2.Y));
}
/// <summary>
/// Calculate Euclidean distance between a point and a line.
/// </summary>
///
/// <param name="point">The point to calculate distance to.</param>
///
/// <returns>Returns the Euclidean distance between this line and the specified point. Unlike
/// <see cref="LineSegment.DistanceToPoint"/>, this returns the distance from the infinite line. (0,0) is 0 units
/// from the line defined by (0,5) and (0,8), but is 5 units from the segment with those endpoints.</returns>
///
public double DistanceToPoint( DoublePoint point )
{
double distance;
if ( !IsVertical )
{
double div = Math.Sqrt( k * k + 1 );
distance = Math.Abs( ( k * point.X + b - point.Y ) / div );
}
else
{
distance = Math.Abs( b - point.X );
}
return distance;
}
/// <summary>
/// Finds intersection point with the specified line.
/// </summary>
///
/// <param name="secondLine">Line to find intersection with.</param>
///
/// <returns>Returns intersection point with the specified line, or
/// <see langword="null"/> if the lines are parallel and distinct.</returns>
///
/// <exception cref="InvalidOperationException">Thrown if the specified line is the same line as this line.</exception>
///
public DoublePoint? GetIntersectionWith( Line secondLine )
{
double k2 = secondLine.k;
double b2 = secondLine.b;
bool isVertical1 = IsVertical;
bool isVertical2 = secondLine.IsVertical;
DoublePoint? intersection = null;
if ( ( k == k2 ) || ( isVertical1 && isVertical2 ) )
{
if ( b == b2 )
{
throw new InvalidOperationException( "Identical lines do not have an intersection point." );
}
}
else
{
if ( isVertical1 )
{
intersection = new DoublePoint( b, k2 * b + b2 );
}
else if ( isVertical2 )
{
intersection = new DoublePoint( b2, k * b2 + b );
}
else
{
// the intersection is at x=(b2-b1)/(k1-k2), and y=k1*x+b1
double x = ( b2 - b ) / ( k - k2 );
intersection = new DoublePoint( x, k * x + b );
}
}
return intersection;
}
/// <summary>
/// A private constructor used only to reuse code inside of this default constructor.
/// </summary>
///
/// <param name="size">Size of points vector to create. This size depends of the shape of the
/// trapezoid.</param>
///
private TrapezoidalFunction( int size )
{
points = new DoublePoint[size];
}
/// <summary>
/// Division operator - divides coordinates of the specified point by scalar value.
/// </summary>
///
/// <param name="point">Point to divide coordinates of.</param>
/// <param name="factor">Division factor.</param>
///
/// <returns>Returns new point which coordinates equal to coordinates of
/// the specified point divided by specified value.</returns>
///
public static DoublePoint Divide( DoublePoint point, double factor )
{
return new DoublePoint( point.X / factor, point.Y / factor );
}
/// <summary>
/// Addition operator - adds values of two points.
/// </summary>
///
/// <param name="point1">First point for addition.</param>
/// <param name="point2">Second point for addition.</param>
///
/// <returns>Returns new point which coordinates equal to sum of corresponding
/// coordinates of specified points.</returns>
///
public static DoublePoint Add( DoublePoint point1, DoublePoint point2 )
{
return new DoublePoint( point1.X + point2.X, point1.Y + point2.Y );
}
/// <summary>
/// Finds local extremum points in the contour.
/// </summary>
/// <param name="contour">A list of <see cref="IntPoint">
/// integer points</see> defining the contour.</param>
///
public List<IntPoint> FindPeaks(List<IntPoint> contour)
{
double[] map = new double[contour.Count];
for (int i = 0; i < contour.Count; i++)
{
IntPoint a, b, c;
int ai = Accord.Math.Tools.Mod(i + K, contour.Count);
int ci = Accord.Math.Tools.Mod(i - K, contour.Count);
a = contour[ai];
b = contour[i];
c = contour[ci];
// http://stackoverflow.com/questions/3486172/angle-between-3-points/3487062#3487062
//double angle = AForge.Math.Geometry.GeometryTools.GetAngleBetweenVectors(b, a, c);
DoublePoint ab = new DoublePoint(b.X - a.X, b.Y - a.Y);
DoublePoint cb = new DoublePoint(b.X - c.X, b.Y - c.Y);
double angba = System.Math.Atan2(ab.Y, ab.X);
double angbc = System.Math.Atan2(cb.Y, cb.X);
double rslt = angba - angbc;
if (rslt < 0)
{
rslt = 2 * Math.PI + rslt;
}
double rs = (rslt * 180) / Math.PI;
if (Theta.IsInside(rs))
map[i] = rs;
}
// Non-Minima Suppression
int r = Suppression;
List<IntPoint> peaks = new List<IntPoint>();
for (int i = 0; i < map.Length; i++)
{
double current = map[i];
if (current == 0) continue;
bool isMinimum = true;
for (int j = -r; j < r && isMinimum; j++)
{
int index = Accord.Math.Tools.Mod(i + j, map.Length);
double candidate = map[index];
if (candidate == 0)
continue;
if (candidate < current)
isMinimum = false;
else map[index] = 0;
}
if (isMinimum)
peaks.Add(contour[i]);
}
return peaks;
}
/// <summary>
/// Addition operator - adds scalar to the specified point.
/// </summary>
///
/// <param name="point">Point to increase coordinates of.</param>
/// <param name="valueToAdd">Value to add to coordinates of the specified point.</param>
///
/// <returns>Returns new point which coordinates equal to coordinates of
/// the specified point increased by specified value.</returns>
///
public static DoublePoint Add( DoublePoint point, double valueToAdd )
{
return new DoublePoint( point.X + valueToAdd, point.Y + valueToAdd );
}
/// <summary>
/// Subtraction operator - subtracts values of two points.
/// </summary>
///
/// <param name="point1">Point to subtract from.</param>
/// <param name="point2">Point to subtract.</param>
///
/// <returns>Returns new point which coordinates equal to difference of corresponding
/// coordinates of specified points.</returns>
///
public static DoublePoint Subtract(DoublePoint point1, DoublePoint point2)
{
return(new DoublePoint(point1.X - point2.X, point1.Y - point2.Y));
}
/// <summary>
/// Multiplication operator - multiplies coordinates of the specified point by scalar value.
/// </summary>
///
/// <param name="point">Point to multiply coordinates of.</param>
/// <param name="factor">Multiplication factor.</param>
///
/// <returns>Returns new point which coordinates equal to coordinates of
/// the specified point multiplied by specified value.</returns>
///
public static DoublePoint Multiply( DoublePoint point, double factor )
{
return new DoublePoint( point.X * factor, point.Y * factor );
}
/// <summary>
/// Addition operator - adds scalar to the specified point.
/// </summary>
///
/// <param name="point">Point to increase coordinates of.</param>
/// <param name="valueToAdd">Value to add to coordinates of the specified point.</param>
///
/// <returns>Returns new point which coordinates equal to coordinates of
/// the specified point increased by specified value.</returns>
///
public static DoublePoint Add(DoublePoint point, double valueToAdd)
{
return(new DoublePoint(point.X + valueToAdd, point.Y + valueToAdd));
}
/// <summary>
/// Calculate squared Euclidean distance between two points.
/// </summary>
///
/// <param name="anotherPoint">Point to calculate distance to.</param>
///
/// <returns>Returns squared Euclidean distance between this point and
/// <paramref name="anotherPoint"/> points.</returns>
///
public double SquaredDistanceTo( DoublePoint anotherPoint )
{
double dx = X - anotherPoint.X;
double dy = Y - anotherPoint.Y;
return dx * dx + dy * dy;
}
private Line( double radius, double theta, bool unused )
{
if ( radius < 0 )
{
throw new ArgumentOutOfRangeException( "radius", radius, "Must be non-negative" );
}
theta *= Math.PI / 180;
double sine = Math.Sin( theta ), cosine = Math.Cos( theta );
DoublePoint pt1 = new DoublePoint( radius * cosine, radius * sine );
// -1/tan, to get the slope of the line, and not the slope of the normal
k = -cosine / sine;
if ( !Double.IsInfinity( k ) )
{
b = pt1.Y - k * pt1.X;
}
else
{
b = Math.Abs( radius );
}
}
/// <summary>
/// Check if the specified set of points form a circle shape.
/// </summary>
///
/// <param name="edgePoints">Shape's points to check.</param>
/// <param name="center">Receives circle's center on successful return.</param>
/// <param name="radius">Receives circle's radius on successful return.</param>
///
/// <returns>Returns <see langword="true"/> if the specified set of points form a
/// circle shape or <see langword="false"/> otherwise.</returns>
///
/// <remarks><para><note>Circle shape must contain at least 8 points to be recognized.
/// The method returns <see langword="false"/> always, of number of points in the specified
/// shape is less than 8.</note></para></remarks>
///
public bool IsCircle( List<IntPoint> edgePoints, out DoublePoint center, out double radius )
{
// make sure we have at least 8 points for curcle shape
if ( edgePoints.Count < 8 )
{
center = new DoublePoint( 0, 0 );
radius = 0;
return false;
}
// get bounding rectangle of the points list
IntPoint minXY, maxXY;
PointsCloud.GetBoundingRectangle( edgePoints, out minXY, out maxXY );
// get cloud's size
IntPoint cloudSize = maxXY - minXY;
// calculate center point
center = minXY + (DoublePoint) cloudSize / 2;
radius = ( (float) cloudSize.X + cloudSize.Y ) / 4;
// calculate mean distance between provided edge points and estimated circle’s edge
float meanDistance = 0;
for ( int i = 0, n = edgePoints.Count; i < n; i++ )
{
meanDistance += (float) Math.Abs( center.DistanceTo( edgePoints[i] ) - radius );
}
meanDistance /= edgePoints.Count;
float maxDitance = Math.Max( minAcceptableDistortion,
( (float) cloudSize.X + cloudSize.Y ) / 2 * relativeDistortionLimit );
return ( meanDistance <= maxDitance );
}
public void EuclideanNormTest( double x, double y, double expectedNorm )
{
DoublePoint point = new DoublePoint( x, y );
Assert.AreEqual( point.EuclideanNorm( ), expectedNorm );
}
