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 ); }