/// <summary>
/// Calculates the adjusted heading based on the current position in relation to the colliding wall.
/// </summary>
/// <param name="currentHeading">The current heading of the navigator.</param>
/// <param name="currentPosition">The current position of the navigator.</param>
/// <returns>The updated navigator heading.</returns>
public double CalculateAdjustedHeading(double currentHeading, DoublePoint currentPosition)
{
double adjustedHeading = 0;
// Calculate the closest point on the colliding wall from the current position
DoublePoint closestPointOnWall = DoubleLine.CalculateLineSegmentClosestPoint(WallLine, currentPosition);
// Adjust the heading based on whether the navigator is approaching from the left or the right
adjustedHeading = currentPosition.X < closestPointOnWall.X
? currentHeading + _leftApproachAdjustment
: currentHeading + _rightApproachAdjustment;
// If the navigator's resulting heading is greater than 360 degrees,
// it has performed more than a complete rotation, so subtract 360
// degrees to have a valid heading
if (adjustedHeading > 360)
{
adjustedHeading -= 360;
}
// On the other hand, if the heading is negative, the same has happened
// in the other direction. So add 360 degrees
else if (adjustedHeading < 0)
{
adjustedHeading += 360;
}
return adjustedHeading;
}
public override DoublePoint GetGoal()
{
WaypointNode targetNode = _path[0];
DoublePoint targetNodePos = new DoublePoint(targetNode.X, targetNode.Y);
// minion at target, advancec target
if (_owner.Pos == targetNodePos)
{
_path.RemoveAt(0);
targetNode = _path[0];
targetNodePos = new DoublePoint(targetNode.X, targetNode.Y);
}
return targetNodePos;
}
/// <summary>
/// Updates each of the range finders based on obstacles along their trajectory. This amounts to setting the output of
/// the range finder to either the distance to the nearest obstacle or, if there are no obstacles in its path, to the
/// maximum distance of the range finder.
/// </summary>
/// <param name="walls">The list of walls in the environment.</param>
/// <param name="heading">The heading of the navigator (in degrees).</param>
/// <param name="location">The location of the navigator in the environment.</param>
internal void Update(List<Wall> walls, double heading, DoublePoint location)
{
// Convert rangefinder angle to radians
var radianAngle = MathUtils.toRadians(Angle);
// Project a point from the navigator location outward
var projectedPoint = new DoublePoint(location.X + Math.Cos(radianAngle)*Range,
location.Y + Math.Sin(radianAngle)*Range);
// Rotate the point based on the navigator's heading
projectedPoint.RotatePoint(heading, location);
// Create a line segment from the navigator's current location to the
// projected point
var projectedLine = new DoubleLine(location, projectedPoint);
// Initialize the range to the maximum range of the range finder sensor
var adjustedRange = Range;
foreach (var wall in walls)
{
// Initialize the intersection indicator to false
var intersectionFound = false;
// Get the intersection point between wall and projected trajectory
// (if one exists)
var wallIntersectionPoint = DoubleLine.CalculateLineIntersection(wall.WallLine, projectedLine,
out intersectionFound);
// If trajectory intersects with a wall, adjust the range to the point
// of intersection (as the range finder cannot penetrate walls)
if (intersectionFound)
{
// Get the distance from the wall
var wallRange = DoublePoint.CalculateEuclideanDistance(wallIntersectionPoint, location);
// If the current wall range is shorter than the current adjusted range,
// update the adjusted range to the shorter value
if (wallRange < adjustedRange)
{
adjustedRange = wallRange;
}
}
}
// Update the range finder range to be the adjusted range
Output = adjustedRange;
}
public bool insideTriangle(DoublePoint inside)
{
Vector2 vec2 = makeVector(corners[0], inside);
float dot00 = dotProduct(vec0, vec0);
float dot01 = dotProduct(vec0, vec1);
float dot02 = dotProduct(vec0, vec2);
float dot11 = dotProduct(vec1, vec1);
float dot12 = dotProduct(vec1, vec2);
double invDenom = 1 / (dot00 * dot11 - dot01 * dot01);
double u = (dot11 * dot02 - dot01 * dot12) * invDenom;
double v = (dot00 * dot12 - dot01 * dot02) * invDenom;
return (u > 0.0) && (v > 0.0) && (u + v < 1);
}
public DoublePoint ProjectionOf(DoublePoint p)
{
double lat = p.Y;
double lon = p.X;
lat *= Math.PI / 180.0;
lat = Math.Log((1.0 + Math.Sin(lat)) / Math.Cos(lat), Math.E);
lat *= 180.0 / Math.PI;
// clamp down the the latitude since it gets really elongated near the poles.
// just picks values that look good
if (lat < -200) lat = -200;
if (lat > 180) lat = 180;
return new DoublePoint(p.X, lat);
}
public static ObservableCollection<DoublePoint> EEG(int count, ref double startPhase, double phaseStep)
{
var doublePoints = new ObservableCollection<DoublePoint>();
var rand = new Random((int)DateTime.Now.Ticks);
for (int i = 0; i < count; i++)
{
var doublePoint = new DoublePoint();
var time = i / (double)count;
doublePoint.Data = time;
doublePoint.Value = 0.05 * (rand.NextDouble() - 0.5) + 1.0;
doublePoints.Add(doublePoint);
startPhase += phaseStep;
}
return doublePoints;
}
/// <summary>
/// Updates each radar in the array based on the given navigator heading and the goal location.
/// </summary>
/// <param name="heading">The heading of the navigator to which the radar array is attached.</param>
/// <param name="location">The location of the goal.</param>
/// <param name="targetLocation">The location of the target (goal).</param>
internal void UpdateRadarArray(double heading, DoublePoint location, DoublePoint targetLocation)
{
var target = targetLocation;
// Rotate the target with respect to the heading of the navigator
target.RotatePoint(-heading, location);
// Offset by the navigator's current location
target.X -= location.X;
target.Y -= location.Y;
// Get the angle between the navigator and the target
var navigatorTargetAngle = DoublePoint.CalculateAngleFromOrigin(target);
// Update every radar in the array based on target alignment
foreach (var radar in Radars)
{
radar.UpdateRadar(navigatorTargetAngle);
}
}
/// <summary>
/// Creates a MazeNavigator with the given starting location. Also sets up the range finders and radar array for the
/// navigator.
/// </summary>
/// <param name="location">The starting location of the navigator.</param>
public MazeNavigator(DoublePoint location)
{
Heading = 0;
Speed = 0;
AngularVelocity = 0;
Location = location;
RangeFinders = new List<RangeFinder>(6)
{
new RangeFinder(100, -180, 0),
new RangeFinder(100, -90, 0),
new RangeFinder(100, -45, 0),
new RangeFinder(100, 0, 0),
new RangeFinder(100, 45, 0),
new RangeFinder(100, 90, 0)
};
RadarArray = new PieSliceSensorArray();
}
public static ObservableCollection<DoublePoint> SquirlyWave()
{
var doublePoints = new ObservableCollection<DoublePoint>();
var rand = new Random((int)DateTime.Now.Ticks);
const int COUNT = 1000;
for (int i = 0; i < COUNT; i++)
{
var doublePoint = new DoublePoint();
var time = i / (double)COUNT;
doublePoint.Data = time;
doublePoint.Value = time * Math.Sin(2 * Math.PI * i / (double)COUNT) +
0.2 * Math.Sin(2 * Math.PI * i / (COUNT / 7.9)) +
0.05 * (rand.NextDouble() - 0.5) +
1.0;
doublePoints.Add(doublePoint);
}
return doublePoints;
}
public void MoveTest()
{
List<Wall> walls = new List<Wall>(11)
{
// Boundary walls
new Wall(new DoubleLine(7, 202, 195, 198), -1, -1, 5),
new Wall(new DoubleLine(41, 5, 3, 8), -1, -1, 5),
new Wall(new DoubleLine(3, 8, 4, 49), 1, 1, 5),
new Wall(new DoubleLine(4, 49, 7, 202), 1, 1, 5),
new Wall(new DoubleLine(195, 198, 186, 8), -1, -1, 5),
new Wall(new DoubleLine(186, 8, 39, 5), -1, -1, 5),
// Obstructing walls
new Wall(new DoubleLine(4, 49, 57, 53), 1, 1, 5),
new Wall(new DoubleLine(56, 54, 56, 157), -1, 1, 5),
new Wall(new DoubleLine(57, 106, 158, 162), 1, 1, 5),
new Wall(new DoubleLine(77, 201, 108, 164), -1, -1, 5),
new Wall(new DoubleLine(6, 80, 33, 121), -1, 1, 5),
new Wall(new DoubleLine(192, 146, 87, 91), -1, -1, 5),
new Wall(new DoubleLine(56, 55, 133, 30), 1, 1, 5)
};
//DoublePoint goalLocation = new DoublePoint(270, 100);
DoublePoint goalLocation = new DoublePoint(31, 20);
//MazeNavigator navigator = new MazeNavigator(new DoublePoint(30, 22));
MazeNavigator navigator = new MazeNavigator(new DoublePoint(14.8669777, 190.702774));
int tempBridgingApplications = 0;
navigator.Speed = -3;
navigator.AngularVelocity = -3;
navigator.Heading = 325.075531;
navigator.Move(walls, goalLocation, false, ref tempBridgingApplications);
Console.WriteLine(DoublePoint.CalculateEuclideanDistance(navigator.Location, goalLocation));
}
/// <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>
/// 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>
/// 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);
}
public double ComputeZ(DoublePoint point) => 2 * Math.Pow(point.X, 2);
public abstract void paintStrategy(System.Drawing.Graphics g, DoublePoint scaling);
/// <summary>
/// Calculate Euclidean distance between a point and a finite line segment.
/// </summary>
///
/// <param name="point">The point to calculate the distance to.</param>
///
/// <returns>Returns the Euclidean distance between this line segment and the specified point. Unlike
/// <see cref="Line.DistanceToPoint"/>, this returns the distance from the finite segment. (0,0) is 5 units
/// from the segment (0,5)-(0,8), but is 0 units from the line through those points.</returns>
///
public double DistanceToPoint( DoublePoint point )
{
double segmentDistance;
switch ( LocateProjection( point ) )
{
case ProjectionLocation.RayA:
segmentDistance = point.DistanceTo( start );
break;
case ProjectionLocation.RayB:
segmentDistance = point.DistanceTo( end );
break;
default:
segmentDistance = line.DistanceToPoint( point );
break;
};
return segmentDistance;
}
public FrequencyPoint(DoublePoint doublePoint, Circle[] circles, int frequency)
{
this.doublePoint = doublePoint;
this.circles = circles;
this.frequency = frequency;
}
public double ComputeZ(DoublePoint p) => Math.Sin(p.X + p.Y);
private void ProcessImage(Bitmap bitmap)
{
// lock image
BitmapData bitmapData = bitmap.LockBits(
new Rectangle(0, 0, bitmap.Width, bitmap.Height),
ImageLockMode.ReadWrite, bitmap.PixelFormat);
// step 1 - turn background to black
ColorFiltering colorFilter = new ColorFiltering();
colorFilter.Red = new IntRange(Properties.Settings.Default.camFilterRed1, Properties.Settings.Default.camFilterRed2);
colorFilter.Green = new IntRange(Properties.Settings.Default.camFilterGreen1, Properties.Settings.Default.camFilterGreen2);
colorFilter.Blue = new IntRange(Properties.Settings.Default.camFilterBlue1, Properties.Settings.Default.camFilterBlue2);
colorFilter.FillOutsideRange = Properties.Settings.Default.camFilterOutside;
colorFilter.ApplyInPlace(bitmapData);
// step 2 - locating objects
BlobCounter blobCounter = new BlobCounter();
blobCounter.FilterBlobs = true;
blobCounter.MinHeight = (int)Properties.Settings.Default.camShapeSizeMin * (int)cameraZoom;
blobCounter.MinWidth = (int)Properties.Settings.Default.camShapeSizeMin * (int)cameraZoom;
blobCounter.MaxHeight = (int)Properties.Settings.Default.camShapeSizeMax * (int)cameraZoom;
blobCounter.MaxWidth = (int)Properties.Settings.Default.camShapeSizeMax * (int)cameraZoom;
blobCounter.ProcessImage(bitmapData);
Blob[] blobs = blobCounter.GetObjectsInformation();
bitmap.UnlockBits(bitmapData);
// step 3 - check objects' type and highlight
SimpleShapeChecker shapeChecker = new SimpleShapeChecker();
shapeChecker.MinAcceptableDistortion = (float)Properties.Settings.Default.camShapeDist;
shapeChecker.RelativeDistortionLimit = (float)Properties.Settings.Default.camShapeDistMax;
Graphics g = Graphics.FromImage(bitmap);
Pen yellowPen = new Pen(Color.Yellow, 5); // circles
Pen redPen = new Pen(Color.Red, 10); // circles
Pen greenPen = new Pen(Color.Green, 5); // known triangle
double lowestDistance = xmid;
double distance;
shapeFound = false;
AForge.Point center;
double shapeRadius = 1;
for (int i = 0, n = blobs.Length; i < n; i++)
{
List <IntPoint> edgePoints = blobCounter.GetBlobsEdgePoints(blobs[i]);
System.Single radius;
// is circle ?
// g.DrawPolygon(greenPen, ToPointsArray(edgePoints));
if (Properties.Settings.Default.camShapeCircle && shapeChecker.IsCircle(edgePoints, out center, out radius))
{
shapeFound = true;
distance = center.DistanceTo((AForge.Point)picCenter);
g.DrawEllipse(yellowPen,
(float)(shapeCenter.X - shapeRadius), (float)(shapeCenter.Y - shapeRadius),
(float)(shapeRadius * 2), (float)(shapeRadius * 2));
if (lowestDistance > Math.Abs(distance))
{
lowestDistance = Math.Abs(distance);
shapeCenter = center;
shapeRadius = radius;
}
}
List <IntPoint> corners;
if (Properties.Settings.Default.camShapeRect && shapeChecker.IsQuadrilateral(edgePoints, out corners)) //.IsConvexPolygon
{
IntPoint minxy, maxxy, centxy;
shapeFound = true;
PolygonSubType subType = shapeChecker.CheckPolygonSubType(corners);
g.DrawPolygon(yellowPen, ToPointsArray(corners));
PointsCloud.GetBoundingRectangle(corners, out minxy, out maxxy);
centxy = (minxy + maxxy) / 2;
distance = picCenter.DistanceTo(centxy);// PointsCloud.GetCenterOfGravity(corners));
if (lowestDistance > Math.Abs(distance))
{
lowestDistance = Math.Abs(distance);
shapeCenter = centxy; // PointsCloud.GetCenterOfGravity(corners);
shapeRadius = maxxy.DistanceTo(minxy) / 2; // 50;
}
}
}
if (shapeFound)
{
g.DrawEllipse(redPen,
(float)(shapeCenter.X - shapeRadius * 1.2), (float)(shapeCenter.Y - shapeRadius * 1.2),
(float)(shapeRadius * 2.4), (float)(shapeRadius * 2.4));
}
yellowPen.Dispose();
redPen.Dispose();
greenPen.Dispose();
g.Dispose();
pictureBoxVideo.BackgroundImage = bitmap;
}
public MoveTrait(IItemService itemService, IItem item, DoublePoint start, DoublePoint end)
{
this.itemService = itemService;
this.item = item;
var distance = Distance(start, end) * 4;
lerpX = new LerpValue2((float)start.X, (float)end.X, distance, AnimationLoop.LoopBackAndForth);
lerpY = new LerpValue2((float)start.Y, (float)end.Y, distance, AnimationLoop.LoopBackAndForth);
}
//Static functions to create shapes
public static DoublePoint[] createLine(DoublePoint a, DoublePoint b, int nrOfPoints)
{
return(null);
}
public static bool isAtHeading(CollisionArea area, DoublePoint robotPosition, DoublePoint robotHeading)
{
DoublePoint corner1, corner2;
if (robotPosition.X > area.getX())
{
if (robotPosition.X > area.getRight())
{
if (robotPosition.Y > area.getBottom())
{
corner1 = area.getPoint(4);
corner2 = area.getPoint(12);
}
else if (robotPosition.Y < area.getY())
{
corner1 = area.getPoint(8);
corner2 = area.getPoint(0);
}
else
{
corner1 = area.getPoint(8);
corner2 = area.getPoint(12);
}
}
else
{
if (robotPosition.Y > area.getBottom())
{
corner1 = area.getPoint(4);
corner2 = area.getPoint(8);
}
else
{
corner1 = area.getPoint(12);
corner2 = area.getPoint(0);
}
}
}
else
{
if (robotPosition.Y > area.getBottom())
{
corner1 = area.getPoint(8);
corner2 = area.getPoint(0);
}
else if (robotPosition.Y < area.getY())
{
corner1 = area.getPoint(4);
corner2 = area.getPoint(12);
}
else
{
corner1 = area.getPoint(4);
corner2 = area.getPoint(0);
}
}
//Largest angle
DoublePoint corner1Heading = new DoublePoint(corner1.X - robotPosition.X, corner1.Y - robotPosition.Y);
//Smallest angle
DoublePoint corner2Heading = new DoublePoint(corner2.X - robotPosition.X, corner2.Y - robotPosition.Y);
double cornerAngDiff = Math.Abs(angleDifferance(corner1Heading, corner2Heading));
double corner1AngDiff = Math.Abs(angleDifferance(robotHeading, corner1Heading));
double corner2AngDiff = Math.Abs(angleDifferance(robotHeading, corner2Heading));
area.setHeadingEdges(corner1, corner2);
if (cornerAngDiff > corner1AngDiff && cornerAngDiff > corner2AngDiff && (corner1AngDiff + corner2AngDiff) < PI)
{
return(true);
}
else
{
return(false);
}
}
public static int[][] Rotate(int[][] mat, double alpha, DoublePoint c)
{
return(Rotate(mat, alpha, c, 255));
}
public DoublePoint ProjectionOf(DoublePoint p)
{
return p;
}
public override void paintStrategy(System.Drawing.Graphics g, DoublePoint scaling)
{
}
/// <summary>
/// Initializes a new instance of the <see cref="LineSegment"/> class.
/// </summary>
///
/// <param name="start">Segment's start point.</param>
/// <param name="end">Segment's end point.</param>
///
/// <exception cref="ArgumentException">Thrown if the two points are the same.</exception>
///
public LineSegment( DoublePoint start, DoublePoint end )
{
line = Line.FromPoints( start, end );
this.start = start;
this.end = end;
}
private void KMeanButton_Click(object sender, EventArgs e)
{
if (!int.TryParse(GroupsCount.Text, out int groupsCount))
{
MessageBox.Show("Niepoprawna liczba grup!");
return;
}
else
{
if (groupsCount < 1)
{
MessageBox.Show("Niepoprawna liczba iteracji!");
return;
}
}
if (!int.TryParse(IterationCount.Text, out int iterationsCount))
{
MessageBox.Show("Niepoprawna liczba iteracji!");
return;
}
else
{
if (iterationsCount < 1)
{
MessageBox.Show("Niepoprawna liczba iteracji!");
return;
}
}
wykres_czysc();
FileExtensionMethods.wczytaj_baze_probek_z_tekstem(_valuesFile, _descriptionFile, out List <List <string> > values, out List <bool> areSymbols, out List <string> names);
MainChart.ChartAreas.Add("chart");
var numery_atr = new List <int>();
probki_str_na_liczby(values, numery_atr, out List <List <double> > probki_num);
var points = new List <DoublePoint>();
for (int i = 0; i < probki_num[0].Count; i++)
{
var point = new DoublePoint()
{
X = probki_num[0][i],
Y = probki_num[1][i]
};
points.Add(point);
}
List <DoublePoint> groupCenters = new List <DoublePoint>();
var rand = new Random();
for (int i = 0; i < groupsCount; i++)
{
var centerPoint = points[rand.Next(0, points.Count - 1)];
if (groupCenters.Any(dp => dp.X == centerPoint.X && dp.Y == centerPoint.Y))
{
i--;
continue;
}
groupCenters.Add(centerPoint);
}
Dictionary <DoublePoint, List <DoublePoint> > groups = new Dictionary <DoublePoint, List <DoublePoint> >();
foreach (var groupCenter in groupCenters)
{
groups.Add(groupCenter, new List <DoublePoint>());
}
for (int i = 0; i < iterationsCount; i++)
{
foreach (var point in points)
{
int minimalDestinationGroupIndex = -1;
double minimalDistance = -1;
for (int j = 0; j < groupCenters.Count; j++)
{
if (minimalDestinationGroupIndex == -1)
{
minimalDestinationGroupIndex = 0;
minimalDistance = CountEuclideanDistanceOfPoints(point, groupCenters[j]);
continue;
}
var distanceNow = CountEuclideanDistanceOfPoints(point, groupCenters[j]);
if (distanceNow < minimalDistance)
{
minimalDestinationGroupIndex = j;
minimalDistance = distanceNow;
}
}
groups[groupCenters[minimalDestinationGroupIndex]].Add(point);
}
List <DoublePoint> newGroupCenters = new List <DoublePoint>();
Dictionary <DoublePoint, List <DoublePoint> > newGroups = new Dictionary <DoublePoint, List <DoublePoint> >();
for (int j = 0; j < groups.Count; j++)
{
double xSum = 0;
double ySum = 0;
for (int k = 0; k < groups[groupCenters[j]].Count; k++)
{
xSum += groups[groupCenters[j]][k].X;
ySum += groups[groupCenters[j]][k].Y;
}
var newCenterPoint = new DoublePoint()
{
X = xSum != 0 ? xSum / (double)groups[groupCenters[j]].Count : xSum,
Y = ySum != 0 ? ySum / (double)groups[groupCenters[j]].Count : ySum
};
newGroupCenters.Add(newCenterPoint);
newGroups.Add(newCenterPoint, new List <DoublePoint>());
}
//ZMIANA
double minimalCountGroup = double.MaxValue;
double maximalCountGroup = double.MinValue;
foreach (var group in groups)
{
if (group.Value.Count < minimalCountGroup)
{
minimalCountGroup = group.Value.Count;
}
if (group.Value.Count > maximalCountGroup)
{
maximalCountGroup = group.Value.Count;
}
}
if (1.1 * minimalCountGroup > maximalCountGroup)
{
break;
}
if (i == iterationsCount - 1)
{
break;
}
groupCenters = newGroupCenters;
groups = newGroups;
}
foreach (var group in groups)
{
List <double> xGroup = new List <double>();
List <double> yGroup = new List <double>();
foreach (var point in group.Value)
{
xGroup.Add(point.X);
yGroup.Add(point.Y);
}
wykres_punkty_rysuj(xGroup, yGroup);
}
foreach (var groupCenter in groupCenters)
{
wykres_punkty_rysuj(new List <double>()
{
groupCenter.X
}, new List <double>()
{
groupCenter.Y
});
}
for (int i = MainChart.Series.Count / 2; i < MainChart.Series.Count; i++)
{
MainChart.Series[i].MarkerSize = 20;
}
}
/// <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 void FCMButton_Click(object sender, EventArgs e)
{
if (!int.TryParse(GroupsCount.Text, out int groupsCount))
{
MessageBox.Show("Niepoprawna liczba grup!");
return;
}
else
{
if (groupsCount < 1)
{
MessageBox.Show("Niepoprawna liczba iteracji!");
return;
}
}
if (!int.TryParse(IterationCount.Text, out int iterationsCount))
{
MessageBox.Show("Niepoprawna liczba iteracji!");
return;
}
else
{
if (iterationsCount < 1)
{
MessageBox.Show("Niepoprawna liczba iteracji!");
return;
}
}
if (!double.TryParse(FuzzyValue.Text, out double fuzziness))
{
MessageBox.Show("Niepoprawny stopień rozmycia!");
return;
}
else
{
if (fuzziness < 1)
{
MessageBox.Show("Niepoprawny stopień rozmycia!");
return;
}
}
wykres_czysc();
FileExtensionMethods.wczytaj_baze_probek_z_tekstem(_valuesFile, _descriptionFile, out List <List <string> > values, out List <bool> areSymbols, out List <string> names);
MainChart.ChartAreas.Add("chart");
var numery_atr = new List <int>();
probki_str_na_liczby(values, numery_atr, out List <List <double> > probki_num);
var points = new List <DoublePoint>();
for (int i = 0; i < probki_num[0].Count; i++)
{
var point = new DoublePoint()
{
X = probki_num[0][i],
Y = probki_num[1][i]
};
points.Add(point);
}
List <DoublePoint> groupCenters = new List <DoublePoint>();
var rand = new Random();
for (int i = 0; i < groupsCount; i++)
{
var centerPoint = points[rand.Next(0, points.Count - 1)];
if (groupCenters.Any(dp => dp.X == centerPoint.X && dp.Y == centerPoint.Y))
{
i--;
continue;
}
groupCenters.Add(centerPoint);
}
for (int i = 0; i < iterationsCount; i++)
{
foreach (var point in points)
{
List <double> destinations = new List <double>();
for (int j = 0; j < groupCenters.Count; j++)
{
destinations.Add(CountEuclideanDistanceOfPoints(point, groupCenters[j]));
}
double sumOfAffiliation = 0;
foreach (var destination in destinations)
{
if (destination != 0)
{
sumOfAffiliation += Math.Pow(destination, 1 - fuzziness);
}
}
foreach (var destination in destinations)
{
//ZMIANA
var affiliation = rand.NextDouble();
point.Affiliation.Add(affiliation);
//if (destination == 0)
//{
// point.Affiliation.Add(0);
//}
//else
//{
// point.Affiliation.Add(Math.Pow(destination, 1 - fuzziness) / sumOfAffiliation);
//}
}
double affSum = 0;
for (int a = 0; a < point.Affiliation.Count; a++)
{
affSum += point.Affiliation[a];
}
for (int a = 0; a < point.Affiliation.Count; a++)
{
point.Affiliation[a] /= affSum;
}
}
List <DoublePoint> newGroupCenters = new List <DoublePoint>();
for (int j = 0; j < groupCenters.Count; j++)
{
double affiliationSum = 0;
foreach (var point in points)
{
affiliationSum += Math.Round(Math.Pow(point.Affiliation[j], fuzziness), 9);
}
double newCenterX = 0;
foreach (var point in points)
{
newCenterX += Math.Round(point.X * Math.Pow(point.Affiliation[j], fuzziness), 9);
}
newCenterX = newCenterX / affiliationSum;
double newCenterY = 0;
foreach (var point in points)
{
newCenterY += Math.Round(point.Y * Math.Pow(point.Affiliation[j], fuzziness), 9);
}
newCenterY = newCenterY / affiliationSum;
var newCenterPoint = new DoublePoint()
{
X = newCenterX,
Y = newCenterY
};
newGroupCenters.Add(newCenterPoint);
}
if (i == iterationsCount - 1)
{
break;
}
groupCenters = newGroupCenters;
}
List <double> xGroup = new List <double>();
List <double> yGroup = new List <double>();
foreach (var point in points)
{
xGroup.Add(point.X);
yGroup.Add(point.Y);
}
wykres_punkty_rysuj(xGroup, yGroup);
foreach (var groupCenter in groupCenters)
{
wykres_punkty_rysuj(new List <double>()
{
groupCenter.X
}, new List <double>()
{
groupCenter.Y
});
}
for (int i = MainChart.Series.Count / 2; i < MainChart.Series.Count; i++)
{
MainChart.Series[i].MarkerSize = 20;
}
}
private double CountEuclideanDistanceOfPoints(DoublePoint one, DoublePoint two)
{
var distance = Math.Sqrt((one.X - two.X) * (one.X - two.X) + (one.Y - two.Y) * (one.Y - two.Y));
return(distance);
}
public override void calculateNextMove(DoublePoint robotPosition, double speed, DoublePoint heading, List <Robot> neighbors, out double referenceSpeed, out DoublePoint referenceHeading)
{
referenceHeading = heading;
referenceSpeed = Program.neutralSpeed;
}
/// <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);
}
internal static void AssertPointEquals(DoublePoint expected, DoublePoint actual, double tolerance)
{
Assert.AreEqual(expected.X, actual.X, tolerance);
Assert.AreEqual(expected.Y, actual.Y, tolerance);
}
/// <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);
}
// 1 mm for .4 mm nozzle
public void SetToDefault()
{
FilamentDiameter = 2.89;
ExtrusionMultiplier = 1;
FirstLayerThickness = .3;
LayerThickness = .1;
FirstLayerExtrusionWidth = .8;
ExtrusionWidth = .4;
SupportExtrusionPercent = 100;
NumberOfPerimeters = 2;
NumberOfBottomLayers = 6;
NumberOfTopLayers = 6;
FirstLayerSpeed = 20;
TopInfillSpeed = 20;
BottomInfillSpeed = 20;
SupportMaterialSpeed = 40;
InfillSpeed = 50;
BridgeSpeed = 20;
BridgeFanSpeedPercent = 100;
RetractWhenChangingIslands = true;
RaftFanSpeedPercent = 100;
OutsidePerimeterSpeed = 50;
OutsidePerimeterExtrusionWidth = ExtrusionWidth;
InsidePerimetersSpeed = 50;
TravelSpeed = 200;
FirstLayerToAllowFan = 2;
SkirtDistanceFromObject = 6;
NumberOfSkirtLoops = 1;
NumberOfBrimLoops = 0;
SkirtMinLength = 0;
InfillPercent = 20;
InfillExtendIntoPerimeter = .06;
InfillStartingAngle = 45;
InfillType = ConfigConstants.INFILL_TYPE.GRID;
CenterObjectInXy = true;
PositionToPlaceObjectCenter = new DoublePoint(102.5, 102.5);
BottomClipAmount = 0;
// raft settings
EnableRaft = false;
RaftAirGap = .2; // .2 mm for .4 mm nozzle
SupportAirGap = .3; //
RaftExtraDistanceAroundPart = 5;
SupportType = ConfigConstants.SUPPORT_TYPE.GRID;
GenerateSupport = false;
SupportPercent = 50;
GenerateInternalSupport = true;
GenerateSupportPerimeter = true;
RaftExtruder = -1;
SupportLineSpacing = ExtrusionWidth * 5;
SupportExtruder = -1;
SupportXYDistanceFromObject = .7;
SupportNumberOfLayersToSkipInZ = 1;
SupportInterfaceLayers = 3;
MinimizeSupportColumns = false; // experimental and not working well enough yet
SupportInterfaceExtruder = -1;
RetractionOnTravel = 4.5;
RetractionSpeed = 45;
RetractionOnExtruderSwitch = 14.5;
UnretractExtraOnExtruderSwitch = 0;
MinimumTravelToCauseRetraction = 10;
MinimumExtrusionBeforeRetraction = 0;
WipeShieldDistanceFromObject = 0;
AvoidCrossingPerimeters = true;
WipeTowerSize = 5;
MultiExtruderOverlapPercent = 0;
MinimumLayerTimeSeconds = 5;
MinimumPrintingSpeed = 10;
FanSpeedMinPercent = 100;
FanSpeedMaxPercent = 100;
ContinuousSpiralOuterPerimeter = false;
StartCode =
"M109 S210 ;Heatup to 210C\n" +
"G21 ;metric values\n" +
"G90 ;absolute positioning\n" +
"G28 ;Home\n" +
"G92 E0 ;zero the extruded length\n";
EndCode =
"M104 S0 ;extruder heater off\n" +
"M140 S0 ;heated bed heater off (if you have it)\n" +
"M84 ;steppers off\n";
}
/// <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>
/// 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>
/// Determines whether the newly proposed location will result in a collision.
/// </summary>
/// <param name="newLocation">The location to which the navigator is prepped to move.</param>
/// <param name="walls">The list of walls in the environment.</param>
/// <returns>Whether or not the proposed move will result in a collision.</returns>
private bool IsCollision(DoublePoint newLocation, List<DoubleLine> walls)
{
var doesCollide = false;
// Iterate through all of the walls, determining if the traversal to the
// newly proposed location will result in a collision
foreach (var wall in walls)
{
// If the distance between the wall and the new location is less than
// the radius of the navigator itself, then a collision will occur
if (!(DoubleLine.CalculateEuclideanDistanceFromLineToPoint(wall, newLocation) < Radius)) continue;
doesCollide = true;
break;
}
return doesCollide;
}
/// <summary>
/// Helper function for isectboxtri.
/// </summary>
private static bool isect(DoublePoint[] boxpoints,
DoublePoint[] tripoints,
DoublePoint axis)
{
if (getmin(boxpoints, axis) > getmax(tripoints, axis)) return false;
if (getmax(boxpoints, axis) < getmin(tripoints, axis)) return false;
return true;
}
/// <summary>
/// Moves the navigator to a new location based on its heading, speed, and angular velocity. The point to which it
/// moves is also dictated by the presence of walls that might be obstructing its path.
/// </summary>
/// <param name="walls">The list of walls in the environment.</param>
internal void Move(List<DoubleLine> walls)
{
// Compute angular velocity components
var angularVelocityX = Math.Cos(MathUtils.toRadians(Heading))*Speed;
var angularVelocityY = Math.Sin(MathUtils.toRadians(Heading))*Speed;
// Set the new heading by incrementing by the angular velocity
Heading += AngularVelocity;
// If the navigator's resulting heading is greater than 360 degrees,
// it has performed more than a complete rotation, so subtract 360
// degrees to have a valid heading
if (Heading > 360)
{
Heading -= 360;
}
// On the other hand, if the heading is negative, the same has happened
// in the other direction. So add 360 degrees
else if (Heading < 0)
{
Heading += 360;
}
// Determine the new location, incremented by the X and Y component velocities
var newLocation = new DoublePoint(angularVelocityX + Location.X, angularVelocityY + Location.Y);
// Move the navigator to the new location only if said movement does not
// result in a wall collision
if (IsCollision(newLocation, walls) == false)
{
Location = new DoublePoint(newLocation.X, newLocation.Y);
}
// Update range finders and radar array
UpdateRangeFinders(walls);
RadarArray.UpdateRadarArray(Heading, Location);
}
/// <summary>
/// Helper function for isectboxtri.
/// </summary>
private static double getmax(DoublePoint[] points,
DoublePoint axis)
{
double max = double.MinValue;
for (int ctr = 0; ctr < points.Length; ctr++)
{
double dotprod = points[ctr].Dot(axis);
if (dotprod > max) max = dotprod;
}
return max;
}
public List <DoublePoint> DoIt()
{
//bool flag_change = false;
int movement, i, j, iteration = 0;
double max_curvature;
DoublePoint temp;
double[] curvature;
alpha = new double[no_of_snake_points];
beta = new double[no_of_snake_points];
gamma = new double[no_of_snake_points];
curvature = new double[no_of_snake_points];
termination_point = false;
for (i = 0; i < no_of_snake_points; i++)
{
alpha[i] = 0.05; // 0.001;
beta[i] = 0.4; // 0.4;
gamma[i] = 1; // 100;
avg_distance += find_distance(i, Snake_points[i]);
}
j = no_of_snake_points;
while (!termination_point)
{
movement = 0;
avg_distance = avg_distance / (double)no_of_snake_points;
max_curvature = double.MinValue;
for (i = 0; i < no_of_snake_points; i++)
//int[] v = randperm(no_of_snake_points);
//for (int k = 0; k < no_of_snake_points; k++)
{
//i = v[k];
temp = find_min_energy(i, Snake_points[i], avg_distance);
//flag_change = false;
if (temp != Snake_points[i] &&
temp != Snake_points[(i - 1 + j) % j] &&
temp != Snake_points[(i + 1) % j])
{
Snake_points[i] = temp;
movement++;
}
curvature[i] = find_curvature(i, Snake_points[i]);
if (max_curvature < curvature[i])
{
max_curvature = curvature[i];
}
}
avg_distance = 0.0;
for (i = 0; i < no_of_snake_points; i++)
{
curvature[i] = curvature[i] / max_curvature;
}
for (i = 0; i < no_of_snake_points; i++)
{
DoublePoint p = Snake_points[i];
avg_distance += find_distance(i, p);
if (curvature[i] > threshold_curvature &&
curvature[i] > curvature[(i + 1) % no_of_snake_points] &&
curvature[i] > curvature[(i - 1 + no_of_snake_points) % no_of_snake_points]
/*&& (double)grad_mag[round(p.X)][round(p.Y)] > threshold_image_energy*/)
{
beta[i] = 0;
}
}
if (movement < threshold_movement)
{
termination_point = true;
}
iteration++;
if (iteration > Max_Iterations)
{
termination_point = true;
}
}
return(Snake_points);
}
/// <summary>
/// Computes the dot product between this DoublePoint and
/// p.
/// </summary>
/// <param name="p">The second point.</param>
/// <returns>dot(this, p)</returns>
public double Dot(DoublePoint p)
{
return x * p.x + y * p.y + z * p.z;
}
// Get type of point's projections to this line segment
private ProjectionLocation LocateProjection( DoublePoint point )
{
// Modified from http://www.codeguru.com/forum/showthread.php?t=194400
/* How do I find the distance from a point to a line segment?
Let the point be C (Cx,Cy) and the line be AB (Ax,Ay) to (Bx,By).
Let P be the point of perpendicular projection of C on AB. The parameter
r, which indicates P's position along AB, is computed by the dot product
of AC and AB divided by the square of the length of AB:
(1) AC dot AB
r = ---------
||AB||^2
r has the following meaning:
r=0 P = A
r=1 P = B
r<0 P is on the backward extension of AB (and distance C-AB is distance C-A)
r>1 P is on the forward extension of AB (and distance C-AB is distance C-B)
0<r<1 P is interior to AB (and distance C-AB(segment) is distance C-AB(line))
The length of the line segment AB is computed by:
L = sqrt( (Bx-Ax)^2 + (By-Ay)^2 )
and the dot product of two 2D vectors, U dot V, is computed:
D = (Ux * Vx) + (Uy * Vy)
So (1) expands to:
(Cx-Ax)(Bx-Ax) + (Cy-Ay)(By-Ay)
r = -------------------------------
(Bx-Ax)^2 + (By-Ay)^2
*/
// the above is modified here to compare the numerator and denominator, rather than doing the division
DoublePoint abDelta = end - start;
DoublePoint acDelta = point - start;
double numerator = acDelta.X * abDelta.X + acDelta.Y * abDelta.Y;
double denomenator = abDelta.X * abDelta.X + abDelta.Y * abDelta.Y;
ProjectionLocation result = ( numerator < 0 ) ? ProjectionLocation.RayA : ( numerator > denomenator ) ? ProjectionLocation.RayB : ProjectionLocation.SegmentAB;
return result;
}
DoublePoint find_min_energy(int no, DoublePoint point, double avg_distance)
{
DoublePoint p, min_point;
p = new DoublePoint();
min_point = new DoublePoint();
double max_curvature, max_continuity, max_internal, min_internal, min_energy, energy;
double[,] curvatures = new double[5, 5];
double[,] continuities = new double[5, 5];
double[,] internal_energies = new double[5, 5];
int i, j, limit = 1;
max_curvature = max_continuity = max_internal = double.MinValue;
min_internal = double.MaxValue;
if (!neighbor3by3)
{
limit++;
}
for (i = -limit; i <= limit; i++)
{
p.Y = point.Y + i;
if (p.Y < 0)
{
p.Y = 0;
}
if (p.Y >= m_Rows)
{
p.Y = m_Rows - 1;
}
for (j = -limit; j <= limit; j++)
{
p.X = point.X + j;
if (p.X < 0)
{
p.X = 0;
}
if (p.X >= m_Cols)
{
p.X = m_Cols - 1;
}
curvatures[i + limit, j + limit] = find_curvature(no, p);
//This code can cause problem near
continuities[i + limit, j + limit] = find_continuity(no, p, avg_distance);
//border of image
//internal_energies[i + limit, j + limit] = (double)grad_mag[round(p.X)][round(p.Y)];
internal_energies[i + limit, j + limit] = GetGVFValue(round(p.X), round(p.Y));
if (curvatures[i + limit, j + limit] > max_curvature)
{
max_curvature = curvatures[i + limit, j + limit];
}
if (continuities[i + limit, j + limit] > max_continuity)
{
max_continuity = continuities[i + limit, j + limit];
}
if (internal_energies[i + limit, j + limit] > max_internal)
{
max_internal = internal_energies[i + limit, j + limit];
}
if (internal_energies[i + limit, j + limit] < min_internal)
{
min_internal = internal_energies[i + limit, j + limit];
}
}
}
for (i = 0; i <= 2 * limit; i++)
{
for (j = 0; j <= 2 * limit; j++)
{
curvatures[i, j] = curvatures[i, j] / max_curvature;
continuities[i, j] = continuities[i, j] / max_continuity;
if ((max_internal - min_internal) < 5)
{
min_internal = max_internal - 5;
}
internal_energies[i, j] = (internal_energies[i, j] - min_internal) / (max_internal - min_internal);
}
}
min_point.X = -limit;
min_point.Y = -limit;
min_energy = double.MaxValue;
for (i = -limit; i <= limit; i++)
{
for (j = -limit; j <= limit; j++)
{
energy = alpha[no] * continuities[i + limit, j + limit]
+ beta[no] * curvatures[i + limit, j + limit]
- gamma[no] * internal_energies[i + limit, j + limit];
if (energy < min_energy || (energy == min_energy && i == 0 && j == 0))
{
min_energy = energy;
min_point.X = j;
min_point.Y = i;
}
}
}
min_point.X = min_point.X + point.X;
min_point.Y = min_point.Y + point.Y;
return(min_point);
}
/*
*
* FOLLOWING METHODS IS USED BY CALIBRATION ONLY
*
*/
/*
* Method that calibrates (aligns) the cameras. This means placing them in a global coordinate system.
* This is done by giving them a base coordinate (upper left coordinate, since X-axis directed right, Y-axis directed down),
* a rotation and a size scaling. The base coordinate defines the cameras translation (offset position).
* The rotation is how much the camera needs to be rotated (in radians) to match the other cameras.
* The size scaling is how much the camera needs to be resized to match the other cameras where scaling = 1 means no resizing.
*/
private void setCameraCoord(object sender, EventArgs e)
{
//Turns off the timer since this method only should be called once.
((Timer)sender).Dispose();
List <Camera> activeCameras = Program.cameraController.getIncludedCameras();
//firstCamera should be the upper left camera in the global coordinate system (X-axis directed right, Y-axis directed down)
//with base coordinates(upper left) = (0,0), rotation = 0 and size scaling = 1. upperLeftCamera, upperCamera and leftCamera are
//temporary cameras used to determine the firstCamera. They are all initialized do the first active camera.
Camera firstCamera = activeCameras[0], upperLeftCamera = activeCameras[0], upperCamera = activeCameras[0], leftCamera = activeCameras[0];
bool upperLeftCameraFound = false, upperCameraFound = false, leftCameraFound = false;
//Calibration is always done in full HD to get maximum precision. But if the prefered resolution in the robot platform is
//lower than full HD, "calibrationScaling" is used to convert correct values.
double calibrationScaling = (double)Program.resolution.Y / (double)Program.calibrationResolution.Y;
//When calibrating a camera, glyphs could be placed at max 4 positions, one in each image quadrant.
//This loop finds the glyphs in each camera and saves their coordinats in camCoordUpperLeft, camCoordUpperRight
//camCoordLowerLeft or camCoordLowerRight depending on in what quadrant of the camera image they are found.
for (int i = 0; i < activeCameras.Count; i++)
{
Camera cam = activeCameras[i];
//Recieves all glyphs found in the camera image
List <ExtractedGlyphData> glyphs = Program.imageProcessing.GetGlyphPosition(CameraController.GrabOneFrame(cam));
foreach (ExtractedGlyphData glyphData in glyphs)
{
IntPoint[] glyphCorners = new IntPoint[4];
//Recieves the coordinates of the 4 corners of the current glyph
glyphData.Quadrilateral.CopyTo(glyphCorners);
//Calculates the center coordinate of the glyph
DoublePoint glyphCoord = (DoublePoint)(glyphCorners[0] + glyphCorners[1] + glyphCorners[2] + glyphCorners[3]) / 4;
int glyphID = Convert.ToInt32(glyphData.RecognizedGlyph.Name);
if (glyphCoord.X <= Program.resolution.X * 0.5)
{
//Glyph found in 2nd quadrant
if (glyphCoord.Y <= Program.resolution.Y * 0.5)
{
cam.camCoordUpperLeft = glyphCoord;
cam.IdUpperLeft = glyphID;
}
//Glyph found in 3rd quadrant
else if (glyphCoord.Y >= Program.resolution.Y * 0.5)
{
cam.camCoordLowerLeft = glyphCoord;
cam.IdLowerLeft = glyphID;
}
}
else if (glyphCoord.X >= Program.resolution.X * 0.5)
{
//Glyph found in 1st quadrant
if (glyphCoord.Y <= Program.resolution.Y * 0.5)
{
cam.camCoordUpperRight = glyphCoord;
cam.IdUpperRight = glyphID;
}
//Glyph found in 4th quadrant
else if (glyphCoord.Y >= Program.resolution.Y * 0.5)
{
cam.camCoordLowerRight = glyphCoord;
cam.IdLowerRight = glyphID;
}
}
}
MessageBox.Show("Cam number: " + cam.camNumber + "\r\n" +
"UpperLeft: " + cam.camCoordUpperLeft.ToString() + " " + "ID: " + cam.IdUpperLeft + "\r\n" +
"UpperRight: " + cam.camCoordUpperRight.ToString() + " " + "ID: " + cam.IdUpperRight + "\r\n" +
"LowerLeft: " + cam.camCoordLowerLeft.ToString() + " " + "ID: " + cam.IdLowerLeft + "\r\n" +
"LowerRight: " + cam.camCoordLowerRight.ToString() + " " + "ID: " + cam.IdLowerRight);
//Determines if the camera is a middle camera, upper left camera, upper camera or left camera.
//Id = -1 means that no glyphs where found in that quadrant
if (cam.IdUpperLeft != -1 && cam.IdUpperRight != -1 && cam.IdLowerLeft != -1 && cam.IdLowerRight != -1)
{
//Middle camera found
}
else if (cam.IdUpperRight != -1 && cam.IdLowerLeft != -1 && cam.IdLowerRight != -1)
{
upperLeftCameraFound = true;
upperLeftCamera = cam;
}
else if (cam.IdLowerLeft != -1 && cam.IdLowerRight != -1)
{
upperCameraFound = true;
upperCamera = cam;
}
else if (cam.IdUpperRight != -1 && cam.IdLowerRight != -1)
{
leftCameraFound = true;
leftCamera = cam;
}
}
//Determines the first camera.
if (upperLeftCameraFound)
{
firstCamera = upperLeftCamera;
}
else if (upperCameraFound)
{
firstCamera = upperCamera;
}
else if (leftCameraFound)
{
firstCamera = leftCamera;
}
else
{
MessageBox.Show("No first camera was found!\r\nRearrange the glyphs and try again!");
}
List <Camera> calibratedCams = new List <Camera>();
List <Camera> unCalibratedCams = new List <Camera>();
//Places the first camera in calibratedCams, which means that the camera has been calibrated and
//placed in the global coordinate system. Also sets the calibration parameters of the first camera.
calibratedCams.Add(firstCamera);
calibratedCams[0].angleToRef = 0;
calibratedCams[0].sizeScaling = 1;
calibratedCams[0].camPosition = new DoublePoint(0, 0);
//Places the rest of the cameras in unCalibratedCams
unCalibratedCams = activeCameras.Except <Camera>(calibratedCams).ToList <Camera>();
//Determines the calibration parameters of the cameras, i.e placing them in the global coordinate system and
//adding them to the sortedCamList.
for (int i = 0; i < calibratedCams.Count; i++)
{
Camera cam1 = calibratedCams[i];
for (int j = 0; j < unCalibratedCams.Count; j++)
{
Camera cam2 = unCalibratedCams[j];
//Uncalibrated cam2 is above cam1
if (cam1.IdUpperLeft == cam2.IdLowerLeft && cam1.IdUpperRight == cam2.IdLowerRight &&
cam1.IdUpperLeft != -1 && cam2.IdLowerLeft != -1 && cam1.IdUpperRight != -1 && cam2.IdLowerRight != -1)
{
cam2.angleToRef = angleDiff(cam1.camCoordUpperLeft, cam1.camCoordUpperRight, cam2.camCoordLowerLeft, cam2.camCoordLowerRight);
cam2.sizeScaling = distDiff(cam1.camCoordUpperLeft, cam1.camCoordUpperRight, cam2.camCoordLowerLeft, cam2.camCoordLowerRight);
cam2.camPosition = ImageProcessing.translateNewPosition(new DoublePoint(0, 0), cam2.camCoordLowerLeft, cam1.camCoordUpperLeft, cam2.angleToRef, cam2.sizeScaling);
cam2.camCoordUpperLeft = ImageProcessing.translateCoordToGlobal(cam2.camCoordUpperLeft, cam2);
cam2.camCoordUpperRight = ImageProcessing.translateCoordToGlobal(cam2.camCoordUpperRight, cam2);
cam2.camCoordLowerLeft = ImageProcessing.translateCoordToGlobal(cam2.camCoordLowerLeft, cam2);
cam2.camCoordLowerRight = ImageProcessing.translateCoordToGlobal(cam2.camCoordLowerRight, cam2);
calibratedCams.Add(cam2);
unCalibratedCams.Remove(cam2);
j--;
}
//Uncalibrated cam2 is below cam1
else if (cam1.IdLowerLeft == cam2.IdUpperLeft && cam1.IdLowerRight == cam2.IdUpperRight &&
cam1.IdLowerLeft != -1 && cam2.IdUpperLeft != -1 && cam1.IdLowerRight != -1 && cam2.IdUpperRight != -1)
{
cam2.angleToRef = angleDiff(cam1.camCoordLowerLeft, cam1.camCoordLowerRight, cam2.camCoordUpperLeft, cam2.camCoordUpperRight);
cam2.sizeScaling = distDiff(cam1.camCoordLowerLeft, cam1.camCoordLowerRight, cam2.camCoordUpperLeft, cam2.camCoordUpperRight);
cam2.camPosition = ImageProcessing.translateNewPosition(new DoublePoint(0, 0), cam2.camCoordUpperLeft, cam1.camCoordLowerLeft, cam2.angleToRef, cam2.sizeScaling);
cam2.camCoordUpperLeft = ImageProcessing.translateCoordToGlobal(cam2.camCoordUpperLeft, cam2);
cam2.camCoordUpperRight = ImageProcessing.translateCoordToGlobal(cam2.camCoordUpperRight, cam2);
cam2.camCoordLowerLeft = ImageProcessing.translateCoordToGlobal(cam2.camCoordLowerLeft, cam2);
cam2.camCoordLowerRight = ImageProcessing.translateCoordToGlobal(cam2.camCoordLowerRight, cam2);
calibratedCams.Add(cam2);
unCalibratedCams.Remove(cam2);
j--;
}
//Uncalibrated cam2 is left of cam1
else if (cam1.IdLowerLeft == cam2.IdLowerRight && cam1.IdUpperLeft == cam2.IdUpperRight &&
cam1.IdLowerLeft != -1 && cam2.IdLowerRight != -1 && cam1.IdUpperLeft != -1 && cam2.IdUpperRight != -1)
{
cam2.angleToRef = angleDiff(cam1.camCoordLowerLeft, cam1.camCoordUpperLeft, cam2.camCoordLowerRight, cam2.camCoordUpperRight);
cam2.sizeScaling = distDiff(cam1.camCoordLowerLeft, cam1.camCoordUpperLeft, cam2.camCoordLowerRight, cam2.camCoordUpperRight);
cam2.camPosition = ImageProcessing.translateNewPosition(new DoublePoint(0, 0), cam2.camCoordLowerRight, cam1.camCoordLowerLeft, cam2.angleToRef, cam2.sizeScaling);
cam2.camCoordUpperLeft = ImageProcessing.translateCoordToGlobal(cam2.camCoordUpperLeft, cam2);
cam2.camCoordUpperRight = ImageProcessing.translateCoordToGlobal(cam2.camCoordUpperRight, cam2);
cam2.camCoordLowerLeft = ImageProcessing.translateCoordToGlobal(cam2.camCoordLowerLeft, cam2);
cam2.camCoordLowerRight = ImageProcessing.translateCoordToGlobal(cam2.camCoordLowerRight, cam2);
calibratedCams.Add(cam2);
unCalibratedCams.Remove(cam2);
j--;
}
//Uncalibrated cam2 is right of cam1
else if (cam1.IdUpperRight == cam2.IdUpperLeft && cam1.IdLowerRight == cam2.IdLowerLeft &&
cam1.IdUpperRight != -1 && cam2.IdUpperLeft != -1 && cam1.IdLowerRight != -1 && cam2.IdLowerLeft != -1)
{
cam2.angleToRef = angleDiff(cam1.camCoordUpperRight, cam1.camCoordLowerRight, cam2.camCoordUpperLeft, cam2.camCoordLowerLeft);
cam2.sizeScaling = distDiff(cam1.camCoordUpperRight, cam1.camCoordLowerRight, cam2.camCoordUpperLeft, cam2.camCoordLowerLeft);
cam2.camPosition = ImageProcessing.translateNewPosition(new DoublePoint(0, 0), cam2.camCoordUpperLeft, cam1.camCoordUpperRight, cam2.angleToRef, cam2.sizeScaling);
cam2.camCoordUpperLeft = ImageProcessing.translateCoordToGlobal(cam2.camCoordUpperLeft, cam2);
cam2.camCoordUpperRight = ImageProcessing.translateCoordToGlobal(cam2.camCoordUpperRight, cam2);
cam2.camCoordLowerLeft = ImageProcessing.translateCoordToGlobal(cam2.camCoordLowerLeft, cam2);
cam2.camCoordLowerRight = ImageProcessing.translateCoordToGlobal(cam2.camCoordLowerRight, cam2);
calibratedCams.Add(cam2);
unCalibratedCams.Remove(cam2);
j--;
}
}
}
//To be able to draw an image of all the cameraimages merged, the size of the combined images needs
//to be determined. This size is saved in imgSize.
foreach (Camera cam in activeCameras)
{
Program.imgSize.X = System.Math.Max((int)cam.camPosition.X + Program.resolution.X, Program.imgSize.X);
Program.imgSize.Y = System.Math.Max((int)cam.camPosition.Y + Program.resolution.Y, Program.imgSize.Y);
}
//Saves the calibration parameters to a save file so you don't have to calebrate the cameras every time.
saveToFile();
//Restore the cameras to their prefered resolution.
Program.cameraController.setCameraResolution(preferedCameraResolution);
Program.cameraCalibrated = true;
Program.cameraController.setShowSmallCameraImage(true);
}
public abstract void calculateNextMove(DoublePoint robotPosition, double speed, DoublePoint heading, List <Robot> neighbors, out double referenceSpeed, out DoublePoint referenceHeading);
/// <summary>
/// Tests whether a triangle intersects an axis-aligned bounding box.
/// Original code by Mike Vandelay
/// </summary>
/// <param name="centerX">The x-component of the box's center</param>
/// <param name="centerY">The y-component of the box's center</param>
/// <param name="centerZ">The z-component of the box's center</param>
/// <param name="boxHalfDim">Half of the box's dimension (our aabb is a cube)</param>
/// <param name="v0">The first triangle vertex</param>
/// <param name="v1">The second triangle vertex</param>
/// <param name="v2">The third triangle vertex</param>
/// <returns>true, iff the triangle intersects the bounding box</returns>
private static bool isectboxtri(double centerX, double centerY, double centerZ,
double boxHalfDim,
Vector3 v0, Vector3 v1, Vector3 v2)
{
int i = 0;
boxpoints[i++] =
new DoublePoint(
centerX + boxHalfDim, centerY + boxHalfDim, centerZ + boxHalfDim);
boxpoints[i++] =
new DoublePoint(
centerX + boxHalfDim, centerY + boxHalfDim, centerZ - boxHalfDim);
boxpoints[i++] =
new DoublePoint(
centerX + boxHalfDim, centerY - boxHalfDim, centerZ + boxHalfDim);
boxpoints[i++] =
new DoublePoint(
centerX + boxHalfDim, centerY - boxHalfDim, centerZ - boxHalfDim);
boxpoints[i++] =
new DoublePoint(
centerX - boxHalfDim, centerY + boxHalfDim, centerZ + boxHalfDim);
boxpoints[i++] =
new DoublePoint(
centerX - boxHalfDim, centerY + boxHalfDim, centerZ - boxHalfDim);
boxpoints[i++] =
new DoublePoint(
centerX - boxHalfDim, centerY - boxHalfDim, centerZ + boxHalfDim);
boxpoints[i++] =
new DoublePoint(
centerX - boxHalfDim, centerY - boxHalfDim, centerZ - boxHalfDim);
i = 0;
tripoints[i++] = new DoublePoint(v0.X, v0.Y, v0.Z);
tripoints[i++] = new DoublePoint(v1.X, v1.Y, v1.Z);
tripoints[i++] = new DoublePoint(v2.X, v2.Y, v2.Z);
// test the x, y, and z axes
if (!isect(boxpoints, tripoints, DoublePoint.UnitX)) return false;
if (!isect(boxpoints, tripoints, DoublePoint.UnitY)) return false;
if (!isect(boxpoints, tripoints, DoublePoint.UnitZ)) return false;
// test the triangle normal
DoublePoint triedge1 = tripoints[1].Sub(tripoints[0]);
DoublePoint triedge2 = tripoints[2].Sub(tripoints[1]);
DoublePoint trinormal = triedge1.Cross(triedge2);
if (!isect(boxpoints, tripoints, trinormal)) return false;
// test the 9 edge cross products
DoublePoint triedge3 = tripoints[0].Sub(tripoints[2]);
if (!isect(boxpoints, tripoints, DoublePoint.UnitX.Cross(triedge1))) return false;
if (!isect(boxpoints, tripoints, DoublePoint.UnitX.Cross(triedge2))) return false;
if (!isect(boxpoints, tripoints, DoublePoint.UnitX.Cross(triedge3))) return false;
if (!isect(boxpoints, tripoints, DoublePoint.UnitY.Cross(triedge1))) return false;
if (!isect(boxpoints, tripoints, DoublePoint.UnitY.Cross(triedge2))) return false;
if (!isect(boxpoints, tripoints, DoublePoint.UnitY.Cross(triedge3))) return false;
if (!isect(boxpoints, tripoints, DoublePoint.UnitZ.Cross(triedge1))) return false;
if (!isect(boxpoints, tripoints, DoublePoint.UnitZ.Cross(triedge2))) return false;
if (!isect(boxpoints, tripoints, DoublePoint.UnitZ.Cross(triedge3))) return false;
return true;
}
// Used by configuration to calculate angle between two cameras
public static double angleDiff(DoublePoint leftUp, DoublePoint leftDown, DoublePoint rightUp, DoublePoint rightDown)
{
double leftAngle = Math.Atan2((leftUp.Y - leftDown.Y), (leftUp.X - leftDown.X));
double rightAngle = Math.Atan2((rightUp.Y - rightDown.Y), (rightUp.X - rightDown.X));
double angleDiff = (leftAngle - rightAngle);
return(angleDiff);
}
/// <summary>
/// Helper function for isectboxtri.
/// </summary>
private static double getmin(DoublePoint[] points,
DoublePoint axis)
{
double min = double.MaxValue;
for (int ctr = 0; ctr < points.Length; ctr++)
{
double dotprod = points[ctr].Dot(axis);
if (dotprod < min) min = dotprod;
}
return min;
}
// Used by configuration to calculate distance between two cameras
public static double distDiff(DoublePoint leftUp, DoublePoint leftDown, DoublePoint rightUp, DoublePoint rightDown)
{
double distLeft = Math.Sqrt(Math.Pow(leftUp.X - leftDown.X, 2) + Math.Pow(leftUp.Y - leftDown.Y, 2));
double distRight = Math.Sqrt(Math.Pow(rightUp.X - rightDown.X, 2) + Math.Pow(rightUp.Y - rightDown.Y, 2));
double sizeChange = distLeft / distRight;
return(sizeChange);
}
/// <summary>
/// Subtracts another DoublePoint from this point.
/// </summary>
/// <param name="p">The vector to subtract from this one.</param>
/// <returns>this - p</returns>
public DoublePoint Sub(DoublePoint p)
{
return new DoublePoint(x - p.x, y - p.y, z - p.z);
}
/// <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 = GABIZ.Base.Mathematic.Tools.Mod(i + K, contour.Count);
int ci = GABIZ.Base.Mathematic.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 = GABIZ.Base.Mathematic.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 = GABIZ.Base.Mathematic.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>
/// Computes the cross product between this DoublePoint and
/// p.
/// </summary>
/// <param name="p">The second point.</param>
/// <returns>cross(this, p)</returns>
public DoublePoint Cross(DoublePoint p)
{
return new DoublePoint(y * p.z - p.y * z,
z * p.x - x * p.z,
x * p.y - y * p.x);
}
double find_continuity(int no, DoublePoint point, double avg_distance)
{
return(System.Math.Pow(avg_distance - find_distance(no, point), 2));
}
