static void PlaneFittingTest()
{
var points = new List <Vector3>();
var actualNormal = new Vector3(2, 7, -4).Normalize();
var actualDistanceFromOrigin = 417.3;
Console.WriteLine("Actual: {0}", actualNormal);
Console.WriteLine("Actual: {0}", actualDistanceFromOrigin);
Console.WriteLine();
var rand = new Random();
var planeCenterPoint = actualNormal * actualDistanceFromOrigin;
var yAxis = Vector3.CrossProduct(Vector3.UnitX, actualNormal).Normalize();
var xAxis = Vector3.CrossProduct(yAxis, actualNormal).Normalize();
for (int i = 0; i < 50000; i++)
{
var xOnPlane = rand.NextDouble() * 50 - 25;
var yOnPlane = rand.NextDouble() * 50 - 25;
var noiseOffPlane = rand.NextDouble() * 10 - 5;
var point = planeCenterPoint + (xOnPlane * xAxis) + (yOnPlane * yAxis) + (noiseOffPlane * actualNormal);
points.Add(point);
}
var plane = GeometricFits.FitPlane(points);
Console.WriteLine("Fit: {0}", plane.Normal);
Console.WriteLine("Fit: {0}", plane.SignedDistanceFromPlane(Vector3.Zero));
}
static void ProfileTest()
{
var spec = PartSpecification.Parse(@"F:\Profiles\511955-002.xml");
foreach (var segment in spec.ProfileSegments)
{
if (segment.IsMasterProfile)
{
var boundary = segment.ToleranceBoundary(0);
boundary.Add(boundary[0]);
using (var writer = System.IO.File.CreateText(@"F:\tolerance.csv"))
{
writer.WriteLine("N, X, Y");
int n = 0;
foreach (var point in boundary)
{
writer.WriteLine("\"{0}\",{1:f5},{2:f5}", n++, point.X, point.Y);
}
}
break;
}
}
List <Vector2> points = new List <Vector2>();
using (var reader = System.IO.File.OpenText(@"F:\Profile378.csv"))
{
reader.ReadLine(); // skip header
while (true)
{
var line = reader.ReadLine();
if (string.IsNullOrWhiteSpace(line))
{
break;
}
var segments = line.Split(',');
double x, y;
if (!double.TryParse(segments[0], out x))
{
throw new ApplicationException();
}
if (!double.TryParse(segments[1], out y))
{
throw new ApplicationException();
}
points.Add(new Vector2(x, y));
}
}
var simplified = AutomationLibrary.Mathematics.Curves.Polyline.Simplify(points, 0.0005);
// identify "blip" if there is one
var findBlip = true;
LineSegment2 blip = null;
if (findBlip)
{
var candidates = new List <Tuple <LineSegment2, double> >(); // candidate segment and score
// let the blip be a short horizontal section much above its surroundings)
foreach (var segment in simplified.AsLineSegments())
{
if (segment.Length > .2)
{
continue;
}
var angle = Math.Abs(segment.Offset.AngleFromXAxis) * 180 / Math.PI;
if (angle > 5)
{
continue;
}
var midX = segment.Midpoint.X;
var midY = segment.Midpoint.Y;
var minX = Math.Min(segment.Origin.X, segment.Destination.X);
var maxX = Math.Max(segment.Origin.X, segment.Destination.X);
var averageBackgroundY = (from p in points
where ((midX - 1) < p.X && p.X < (midX + 1)) && // must be within 1 inch of candidate midpoint
(p.X < minX || maxX < p.X) && // must not be within segment itself
(p.Y > 0.001) // must not be off the wire
select p.Y).Average();
var score = midY - averageBackgroundY;
if (score > 0)
{
candidates.Add(Tuple.Create(segment, score));
}
}
// order candidates by decreasing score
candidates.Sort((c1, c2) => - c1.Item2.CompareTo(c2.Item2));
// limit candidates to a reasonable number to avoid complexity blowup in failure cases
candidates = candidates.Take(10).ToList();
// merge adjacent candidates into one
var result = new List <LineSegment2>();
while (candidates.Count > 0)
{
var newResult = candidates[0].Item1;
candidates.RemoveAt(0);
// we may wish to extend it by merging it with any other top candidates that are adjacent
int i = 0;
while (true)
{
if (i >= candidates.Count)
{
break;
}
var extensionCandidate = candidates[i].Item1;
if (extensionCandidate.Destination.Equals(newResult.Origin))
{
candidates.RemoveAt(i);
newResult = LineSegment2.FromOriginAndDestination(extensionCandidate.Origin, newResult.Destination);
}
else if (newResult.Destination.Equals(extensionCandidate.Origin))
{
candidates.RemoveAt(i);
newResult = LineSegment2.FromOriginAndDestination(newResult.Origin, extensionCandidate.Destination);
}
else
{
i++;
}
}
result.Add(newResult);
}
if (candidates.Count == 0)
{
blip = null;
}
else
{
blip = candidates[0].Item1; // hooray, we found the blip
candidates.RemoveAt(0);
// we may wish to extend it by merging it with any other top candidates that are adjacent
while (candidates.Count > 0)
{
var extensionCandidate = candidates[0].Item1;
candidates.RemoveAt(0);
if (extensionCandidate.Destination.Equals(blip.Origin))
{
blip = LineSegment2.FromOriginAndDestination(extensionCandidate.Origin, blip.Destination);
}
else if (blip.Destination.Equals(extensionCandidate.Origin))
{
blip = LineSegment2.FromOriginAndDestination(blip.Origin, extensionCandidate.Destination);
}
else
{
break;
}
}
}
}
var resimplified = new List <LineSegment2>();
foreach (var seg in simplified.AsLineSegments())
{
var startX = seg.Origin.X;
var endX = seg.Destination.X;
var blipStartX = blip.Origin.X - .05;
var blipEndX = blip.Destination.X + .05;
if (startX < blipStartX || blipEndX < endX)
{
resimplified.Add(seg);
}
}
resimplified.Add(blip);
var resimplifiedPoints = new List <Vector2>();
foreach (var seg in resimplified)
{
if (!resimplifiedPoints.Contains(seg.Origin))
{
resimplifiedPoints.Add(seg.Origin);
}
if (!resimplifiedPoints.Contains(seg.Destination))
{
resimplifiedPoints.Add(seg.Destination);
}
}
resimplifiedPoints.Sort((p1, p2) => p1.X.CompareTo(p2.X));
var fitLines = new List <Tuple <double, double, Line2> >();
for (int i = 0; i < resimplifiedPoints.Count - 1; i++)
{
var start = resimplifiedPoints[i];
var end = resimplifiedPoints[i + 1];
var segPoints = SelectPointsBetween(points, start.X, end.X);
var n = segPoints.Count;
int skip = 0;
if (n > 20)
{
skip = n / 10;
}
else if (n > 4)
{
skip = n / 4;
}
var notNearEnds = segPoints.SkipBothEnds(skip);
var segFineLine = GeometricFits.FitLine(notNearEnds.ToList());
fitLines.Add(Tuple.Create(start.X, end.X, segFineLine));
}
using (var writer = System.IO.File.CreateText(@"F:\profile-fit-lines.csv"))
{
writer.WriteLine("X,Y");
foreach (var point in resimplifiedPoints)
{
writer.WriteLine("{0},{1}", point.X, point.Y);
}
/*
* writer.WriteLine("{0},{1}", fitLines[0].Item1, fitLines[0].Item3.Intercept + fitLines[0].Item3.Slope * fitLines[0].Item1);
*
* for (int i = 0; i < fitLines.Count - 1; i++)
* {
* var cross = Line2.Intersection(fitLines[i].Item3, fitLines[i + 1].Item3);
* if (cross.HasValue)
* {
* writer.WriteLine("{0},{1}", cross.Value.X, cross.Value.Y);
* }
* else throw new Exception();
* }
*
* var lastIdx = fitLines.Count - 1;
* writer.WriteLine("{0},{1}", fitLines[lastIdx].Item2, fitLines[lastIdx].Item3.Intercept + fitLines[lastIdx].Item3.Slope * fitLines[lastIdx].Item2);
*/
}
Console.WriteLine("Profile processed.");
Console.WriteLine();
Console.ReadLine();
}
static void TestEclipsePipe()
{
using (var reader = System.IO.File.OpenText(@"M:\Superior Energy\Test Runs\data_4.dat"))
{
for (int i = 0; i < 8; i++)
{
reader.ReadLine(); // skip header
}
var polarPoints = new List <Vector2>();
while (true)
{
var line = reader.ReadLine();
if (line == null)
{
break;
}
var nums = line.Split('\t');
var laser = double.Parse(nums[1]);
var encoder = (int)double.Parse(nums[2]);
const int EncoderPPR = 4000; // encoder PPR is 1000, but it is a 4x subsampling quadrature encoder
if (encoder < 32000)
{
encoder += 65536; // to allow for rotating wrong way, move things first to account for counter rollover
}
encoder %= EncoderPPR;
if (encoder < 0)
{
encoder += EncoderPPR;
}
var theta = (2 * Math.PI * encoder) / EncoderPPR;
//const double LaserVoltageAtEdgeOfHolder = 1.3612; // as measured by 1-2-3 block method
const double LaserVoltageAtEdgeOfHolder = 1.381; // as empirically derived by assuming theoretical scale factor and pipe size
const double LaserScaleFactorInchesPerVolt = 12.0 / 10.0; // theoretical scale factor from datasheet
const double LaserHolderHalfWidth = 2.3125; // theoretical from drawing, part measures perfectly for width
var r = ((laser - LaserVoltageAtEdgeOfHolder) * LaserScaleFactorInchesPerVolt) + LaserHolderHalfWidth;
polarPoints.Add(new Vector2(theta, r));
}
var cartesianPoints = new List <Vector2>(polarPoints.Count);
foreach (var polar in polarPoints)
{
cartesianPoints.Add(new Vector2(polar.Y * Math.Cos(polar.X), polar.Y * Math.Sin(polar.X)));
}
var circle = GeometricFits.FitCircle(cartesianPoints);
Console.WriteLine("Point Count: {0}", cartesianPoints.Count);
Console.WriteLine();
Console.WriteLine("Radius: {0:f3}", circle.Radius);
Console.WriteLine("Diameter: {0:f3}", circle.Diameter);
Console.WriteLine("Center X: {0:f3}", circle.Center.X);
Console.WriteLine("Center Y: {0:f3}", circle.Center.Y);
Console.WriteLine("Center Offset: {0:f3}", circle.Center.Length);
Console.WriteLine();
var ellipse = GeometricFits.FitEllipse(cartesianPoints);
Console.WriteLine("Elliptical Major Diameter: {0:f3}", ellipse.MajorAxisLength);
Console.WriteLine("Elliptical Minor Diameter: {0:f3}", ellipse.MinorAxisLength);
Console.WriteLine("Elliptical Eccentricity: {0:f4}", ellipse.Eccentricity);
Console.ReadLine();
var censoredCenteredCartesian = new List <Vector2>();
foreach (var point in cartesianPoints)
{
var centered = point - circle.Center;
if (centered.Length > 2.95)
{
censoredCenteredCartesian.Add(point);
}
}
var circle2 = GeometricFits.FitCircle(censoredCenteredCartesian);
Console.WriteLine("Radius: {0:f3}", circle2.Radius);
Console.WriteLine("Diameter: {0:f3}", circle2.Diameter);
Console.WriteLine("Center X: {0:f3}", circle2.Center.X);
Console.WriteLine("Center Y: {0:f3}", circle2.Center.Y);
Console.WriteLine("Center Offset: {0:f3}", circle2.Center.Length);
Console.WriteLine();
Console.ReadLine();
var function = AutomationLibrary.Mathematics.Curves.CircularFunction.FromCartesianPoints(censoredCenteredCartesian);
function = function.SavitzkyGolaySmooth(7, 31);
var area = function.ComputeArea();
Console.WriteLine("Circular area: {0:f2}", circle.Radius * circle.Radius * Math.PI);
Console.WriteLine("Smoothed function area: {0:f2}", area);
Console.ReadLine();
using (var writer = System.IO.File.CreateText(@"M:\Superior Energy\Test Runs\data_4.csv"))
{
writer.WriteLine("X, Y, R, Theta");
foreach (var point in cartesianPoints)
{
var centered = point - circle.Center;
writer.WriteLine("{0},{1},{2},{3}", centered.X, centered.Y, centered.Length, centered.AngleFromXAxis);
}
}
}
}
static void ClowWater()
{
List <Vector3> p3 = new List <Vector3>();
p3.Add(new Vector3(1.1, 0.9, 1.0));
p3.Add(new Vector3(6.9, 7.1, 7.0));
var line3fit = AutomationLibrary.Mathematics.Fitting.GeometricFits.FitLine(p3);
var line3 = Line3.FromPointAndDirection(new Vector3(0.5, 0.5, 0.5), new Vector3(1, 1, 1));
var nearest = line3.GetClosestPoint(new Vector3(27, -1.4, 19));
List <Vector2> points = new List <Vector2>();
using (var reader = System.IO.File.OpenText(@"C:\Users\douglas\desktop\pipe.csv"))
{
reader.ReadLine(); // skip header
while (true)
{
var line = reader.ReadLine();
if (line == null)
{
break;
}
var nums = line.Split(',');
var values = nums.Select(n => double.Parse(n)).ToArray();
points.Add(new Vector2(values[0], values[1]));
}
}
var ellipse = AutomationLibrary.Mathematics.Fitting.GeometricFits.FitEllipse(points);
var ellipseFunc = AutomationLibrary.Mathematics.Curves.CircularFunction.FromCartesianPoints(ellipse.Center, points);
var smoothEllipseFunc = ellipseFunc.SavitzkyGolaySmooth(3, 21);
points.Clear();
points.AddRange(GeneratePointsOnEllipticalArc(new Vector2(0.37, -2.4), 21, 24.26, 96.3 * Math.PI / 180.0, .020, -95.0 * Math.PI / 180.0, 97.0 * Math.PI / 180.0).Take(1000));
var pointSet = new PointCloud2(points);
var voronoi = AutomationLibrary.Mathematics.Geometry.Voronoi.VoronoiDiagram.ComputeForPoints(points);
voronoi = voronoi.Filter(0); // build map of nearest points
var centersOfInfiniteCells = new HashSet <Vector2>();
foreach (var edge in voronoi.Edges)
{
if (edge.IsPartlyInfinite)
{
centersOfInfiniteCells.Add(edge.LeftData);
centersOfInfiniteCells.Add(edge.RightData);
}
}
var pointSet2 = new PointCloud2(centersOfInfiniteCells);
var mcc = pointSet2.ComputeMinimumCircumscribingCircle();
var mic = ComputeMaximumInscribedCircle(pointSet, voronoi, mcc);
var lsc = GeometricFits.FitCircle(points);
using (var writer = System.IO.File.CreateText(@"C:\users\douglas\desktop\circlepoints.csv"))
{
writer.WriteLine("X,Y");
foreach (var point in pointSet)
{
writer.WriteLine("{0},{1}", point.X, point.Y);
}
}
Console.WriteLine("n = {0}", points.Count);
Console.WriteLine("MIC @ ({0}), r = {1}", mic.Center, mic.Radius);
Console.WriteLine("LSC @ ({0}), r = {1}", lsc.Center, lsc.Radius);
Console.WriteLine("MCC @ ({0}), r = {1}", mcc.Center, mcc.Radius);
Console.WriteLine();
Console.WriteLine("draw.circle({0}, {1}, {2}, border='{3}')", mic.Center.X, mic.Center.Y, mic.Radius, "red");
Console.WriteLine("draw.circle({0}, {1}, {2}, border='{3}')", lsc.Center.X, lsc.Center.Y, lsc.Radius, "blue");
Console.WriteLine("draw.circle({0}, {1}, {2}, border='{3}')", mcc.Center.X, mcc.Center.Y, mcc.Radius, "green");
Console.ReadLine();
}
