public static Polygon Create(IPointCloud pointCloud, double searchRange, int kNearestCount)
{
var result = new Polygon();
var currPoint = pointCloud.QuadTree.GetLowestYPoint();
result.Points.Add(currPoint);
//Uno a sinistra così può calcolare l'angolo in modo uniforme
var prevPoint = new DblPoint2(currPoint.x - 1, currPoint.y);
while (true)
{
var nextPoint = GetNextPoint(pointCloud, result, prevPoint, currPoint, result.Points.Count < 3 ? result.Points : result.Points.Skip(1),
searchRange, kNearestCount);
if (nextPoint == result.Points[0])
break;
result.Points.Add(nextPoint);
prevPoint = currPoint;
currPoint = nextPoint;
}
return result;
}
public static CubicBezier Interpolate(DblPoint2 Pa, DblPoint2 P0, DblPoint2 P1, DblPoint2 Pb)
{
var result = new CubicBezier();
result.P0 = P0;
result.P1 = P1;
result.C0 = InterpolateControlPoint(Pa, P0, P1);
result.C1 = InterpolateControlPoint(Pb, P1, P0);
return result;
}
public CCuttingPlaneVM()
{
mPlannedLines = new ObservableCollection<CLineVM>();
mDrawnLines = new ObservableCollection<CLineVM>();
Grbl.Planner.PlannerBlocksChanged += mGrbl_PlannerBlocksChanged;
mCuttingPlaneSize = new DblPoint2(500, 500);
CalculateTransform();
}
private static DblPoint2 InterpolateControlPoint(DblPoint2 Pa, DblPoint2 P0, DblPoint2 P1)
{
//<BlackMagic>
var Ma0 = (Pa + P0) * 0.5;
var M01 = (P0 + P1) * 0.5;
var lenPaP0 = (Pa - P0).Length;
var lenP0P1 = (P0 - P1).Length;
var Q1 = Ma0 + (M01 - Ma0) * (lenPaP0 / (lenPaP0 + lenP0P1));
return P0 + (M01 - Q1);
//</BlackMagic>
}
public void AddPoint(DblPoint2 p)
{
if (mTopLeft == null)
{
mPoints.Add(p);
if (mPoints.Count == PointCapacity && mBounds.Width > MinimunCellWidth)
Split();
}
else
{
var subQT = Classify(p);
subQT.AddPoint(p);
}
}
private static DblPoint2 GetNextPoint(IPointCloud pointCloud, Polygon partialResult, DblPoint2 prevPoint, DblPoint2 currPoint, IEnumerable<DblPoint2> excludedPoints,
double searchRange, int kNearestCount)
{
while (true)
{
var kNearest = new List<DblPoint2>();
var actualkNearestCount = kNearestCount;
while (true)
{
pointCloud.QuadTree.GetPointsInsideCircle(currPoint, 1000000.0, kNearest);
kNearest.RemoveAll((x) => excludedPoints.Contains(x));
if (kNearest.Count < actualkNearestCount && kNearest.Count != (pointCloud.Points.Count() - excludedPoints.Count()))
{
actualkNearestCount++;
kNearest.Clear();
}
else if (kNearest.Count > 3)
{
kNearest = (from k in kNearest
orderby (k - currPoint).Length2 ascending
select k).Take(actualkNearestCount).ToList();
break;
}
else
break;
}
var kNearestOrderedByAngle = (from k in kNearest
orderby (prevPoint - currPoint).CWAngleTo(k - currPoint) descending
select k).ToList();
//Rimuovi tutti quelli che intersecano il poligono corrente
var kNearestOrderedByAngleNotIntersecting = (from k in kNearestOrderedByAngle
where !IntersectAnyPolygonEdgeExceptLast(partialResult, currPoint, k)
select k).ToList();
if (kNearestOrderedByAngleNotIntersecting.Count > 0)
return kNearestOrderedByAngleNotIntersecting.First();
}
}
public double GetDistance(DblPoint2 center)
{
if (mTopLeft == null)
{
var result = double.MaxValue;
foreach (var p in mPoints)
{
var d = (p - center).Length2;
if (d < result)
result = d;
}
return System.Math.Sqrt(result);
}
else
{
var resultTop = System.Math.Min(mTopLeft.GetDistance(center), mTopRight.GetDistance(center));
var resultBottom = System.Math.Min(mBottomLeft.GetDistance(center), mBottomRight.GetDistance(center));
return System.Math.Min(resultTop, resultBottom);
}
}
public static CubicBezier[] Interpolate(DblPoint2[] Points, DblPoint2 prePoint, DblPoint2 postPoint)
{
if (Points.Length < 2)
throw new ArgumentException("Too few points to interpolate");
var beziersNecessary = Points.Length - 1;
var result = new CubicBezier[beziersNecessary];
for (int i = 0; i < beziersNecessary; i++)
{
DblPoint2 Pa, Pb;
if (i == 0)
Pa = prePoint;
else
Pa = Points[i - 1];
if (i == beziersNecessary - 1)
Pb = postPoint;
else
Pb = Points[i + 2];
result[i] = Interpolate(Pa, Points[i], Points[i + 1], Pb);
}
return result;
}
public DblPoint2 Apply(DblPoint2 input)
{
return new DblPoint2(input.x * mScale.x + mTranslate.x,
input.y * mScale.y + mTranslate.y);
}
public Segment(DblPoint2 p1, DblPoint2 p2)
{
this.P1 = p1;
this.P2 = p2;
}
//public double AngleTo(DblPoint2 other)
//{
// return System.Math.Acos(Dot(other) / (Length + other.Length)
//}
public double CWAngleTo(DblPoint2 other)
{
//angle of 2 relative to 1= atan2(v2.y,v2.x) - atan2(v1.y,v1.x)
var result = System.Math.Atan2(y, x) - System.Math.Atan2(other.y, other.x);
//Riporta da 0 a 2pi
if (result > 0)
return result;
else
return result + 2 * System.Math.PI;
}
private static Boolean IntersectAnyPolygonEdgeExceptLast(Polygon polygon, DblPoint2 p1, DblPoint2 p2)
{
var s12 = new Segment(p1, p2);
for (var i = 0; i < polygon.Points.Count - 1; i++)
{
var result = new Segment(polygon.Points[i], polygon.Points[i + 1]).Intersect(s12);
if (result.IsIntersection)
return true;
}
return false;
}
public double Dot(DblPoint2 other)
{
return x * other.x + y * other.y;
}
public void Resize(double newWidth, double newHeight)
{
mCuttingPlaneSize = new DblPoint2(newWidth, newHeight);
CalculateTransform();
mSysPositionTransformed = mWorldToCuttingPlaneTransform.Apply(mSysPosition);
foreach (var lineVM in mPlannedLines)
lineVM.ApplyTransform(mWorldToCuttingPlaneTransform);
foreach (var lineVM in mDrawnLines)
lineVM.ApplyTransform(mWorldToCuttingPlaneTransform);
Notify("CuttingPlaneSizeX");
Notify("CuttingPlaneSizeY");
Notify("CurrX");
Notify("CurrY");
}
public void GetPointsInsideCircle(DblPoint2 center, double radius, List<DblPoint2> result)
{
if (mTopLeft == null)
{
var radius2 = radius * radius;
foreach (var p in mPoints)
if ((p - center).Length2 < radius2)
result.Add(p);
}
else
{
var deltaC = (center - this.Center);
/* |
* |--r>0--
* |
* --------c--------
* |
* |
* |
*/
//Compare with radius instead of with 0, to include points on the right quadrant that are near the border
if (deltaC.x < radius && deltaC.y < radius)
mBottomLeft.GetPointsInsideCircle(center, radius, result);
if (deltaC.x > -radius && deltaC.y < radius)
mBottomRight.GetPointsInsideCircle(center, radius, result);
if (deltaC.x < radius && deltaC.y > -radius)
mTopLeft.GetPointsInsideCircle(center, radius, result);
if (deltaC.x > -radius && deltaC.y > -radius)
mTopRight.GetPointsInsideCircle(center, radius, result);
}
}
public void Update()
{
//SB! Save original coordinates and normalize them in the property getter so that we can dinamically size the
//drawing surface
//mSysPosition = new DblPoint2(LaserCatHardwareSimulator.sys.position[0], LaserCatHardwareSimulator.sys.position[1]);
mSysPosition = new DblPoint2(Grbl.sys.position[0],
Grbl.sys.position[1]);
mSysPositionTransformed = mWorldToCuttingPlaneTransform.Apply(mSysPosition);
Notify("CurrX");
Notify("CurrY");
}
private QuadTree Classify(DblPoint2 p)
{
var deltaC = (p - this.Center);
var isLeft = deltaC.x <= 0;
var isBottom = deltaC.y <= 0;
if (isLeft)
if (isBottom)
return mBottomLeft;
else
return mTopLeft;
else
if (isBottom)
return mBottomRight;
else
return mTopRight;
}
