Inverse Kinematics

import java.util.List;

// Class to calculate inverse kinematics
public class InvKin {

    private double[] lengths;
    private List<Point> points;
    private Point start;
    private Point target;

    public InvKin(List<Point> points, Point target) {
        if (points.size() < 2) throw new IllegalArgumentException("Not enough points for inverse kinematics");
        this.start = points.get(0).copy();
        this.points = points;
        this.target = target;
        //
        lengths = new double[points.size() - 1];
        for (int i = 0; i < lengths.length; i++) {
            lengths[i] = points.get(i).distance(points.get(i + 1));
        }
    }

    private void backwards() {
        int last = points.size() - 1;
        points.get(last).set(target);
        for (int i = points.size() - 2; i >= 0; i--) {
            Point c = points.get(i);
            Point n = points.get(i + 1);
            c.set(n.getPoint(n.angleTo(c), lengths[i]));
        }
    }

    private void forwards() {
        int first = 0;
        points.get(first).set(start);
        for (int i = 1; i < points.size(); i++) {
            Point c = points.get(i);
            Point p = points.get(i - 1);
            c.set(p.getPoint(p.angleTo(c), lengths[i - 1]));
        }
    }

    private void iterate() {
        backwards();
        forwards();
    }

    // Calculate new positions within the error bound.
    public void calculate(double err) {
        int total = 0;
        for (double length : lengths) {
            total += length;
        }
        if (start.distance(target) > total) { // Chain isn't long enough to connect
            for (int i = 1; i < points.size(); i++) {
                Point c = points.get(i);
                Point p = points.get(i - 1);
                c.set(p.getPoint(p.angleTo(target), lengths[i - 1]));
            }
            return;
        }
        int last = points.size() - 1;
        double error = points.get(last).distance(target);
        double l = -1;
        while (error > err) {
            iterate();
            error = points.get(last).distance(target);
            if (l == error) break;
            l = error;
        }
    }

    public List<Point> getPoints() {
        return points;
    }

}