Adaptive Radius

  public class circfill : GoalObject
  {
    double diam;
    double change;
    double prevMin;
    double ratio;
    Curve Crv;

    public circfill(Point3d[] P, Curve C, double k)
    {
      PPos = P;
      Crv = C;
      Move = new Vector3d[P.Length];
      Weighting = new double[P.Length];
      for(int i = 0;i < P.Length;i++)
      {
        Weighting[i] = k;
      }
      change = 0.01;
      prevMin = -1;
    }
    public override void Calculate(List<KangarooSolver.Particle> p)
    {
      diam *= (1 + change);
      Point3d[] pts = this.GetCurrentPositions(p);
      int[][] neighbours = RTree.Point3dKNeighbors(pts, pts, Math.Min(6, pts.Length)).ToArray();
      double minDist = double.MaxValue;
      for (int i = 0; i < neighbours.Length; i++)
      {
        double dist = pts[i].DistanceTo(pts[neighbours[i][1]]);
        if(dist < minDist) minDist = dist;
      }
      if(prevMin == -1) prevMin = minDist;
      for (int i = 0; i < neighbours.Length; i++) Move[i] = Vector3d.Zero;
      int[] hitCount = new int[neighbours.Length];
      for (int i = 0; i < neighbours.Length; i++)
      {
        int[] ni = neighbours[i];
        for(int j = 0;j < ni.Length;j++)
        {
          if(ni[j] > i)
          {
            double dist = pts[i].DistanceTo(pts[ni[j]]);
            Vector3d v = pts[ni[j]] - pts[i];
            double length = v.Length;
            v.Unitize();
            if(length - diam < 0)
            {
              Move[i] += 0.5 * v * (length - diam);
              Move[ni[j]] -= 0.5 * v * (length - diam);
              hitCount[i]++;
              hitCount[ni[j]]++;
            }
          }
        }
        double t;
        if(Crv.ClosestPoint(pts[i], out t, 0.5 * diam))
        {
          Vector3d Push = pts[i] - Crv.PointAt(t);
          double length = Push.Length;          
          Push.Unitize();
          Move[i] -= Push * (length - 0.5 * diam);
          hitCount[i]++;
        }
      }
      for (int i = 0; i < neighbours.Length; i++) if(hitCount[i] > 0) Move[i] *= 1.0 / hitCount[i];
      ratio = minDist / prevMin;
      change = ratio < 1.0 ? change * (0.99) : change + 0.5 * (ratio - 1.0);
      prevMin = minDist;
      diam = minDist;
    }

    public override object Output(List<KangarooSolver.Particle> p)
    {
      return 0.5 * diam;
    }

  }