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