FABRIK Inverse Kinematics

// TODO: finish fixing desmosify function

function getType(obj) {
    if (obj === null) return 'null';
    if (obj === undefined) return 'undefined';
    return obj.__proto__.constructor.name;
}

function from(min) {
    let fromObj = [];
    fromObj.to = (max) => {
        let toObj = {};
        toObj.run = (statement) => {
            if (min > max) throw new RangeError(`loop start must be below loop max`)
            for (let i = min; i <= max; i++) {
                statement(i);
            }
        }
        return toObj;
    }
    return fromObj;
}

function desmosLerp(a,b) {
    if (getType(a) != getType(b)) throw new TypeError(`cannot lerp between a point and a number`);
    if (getType(a) == "Vector2") {
        a = a.toString(10000);
        // a.replace("(", "\\left(");
        // a.replace(")", "\\right)");
        // a.replace(" ", "");
        b = b.toString(10000);
        // b.replace("(", "\\left(");
        // b.replace(")", "\\right)");
        // b.replace(" ", "");
    }
    return `l_{erp}(t,${a},${b})`
}

function desmosify(points, lerp, name) {
    let result = name[0]+"_{"+name.substring(1)+"}=\\left[";

    for (let i = 0; i < points.length; i++) {
        let pt = points[i];
        if (i == points.length-1 && lerp) continue;
        if (lerp) {
            result += desmosLerp(points[i], points[i+1]);
            result += (i != points.length-2 ? ',' : '');
            continue;
        }
        result += pt.toString(10000);
        result += (i != points.length-1 ? ',' : '');
    }
    /*
    points.forEach((pt,i) => {
        if (lerp) {
            result += desmosLerp(pt
        }
        result += pt.toString(10000);
        result += (i != points.length-1 ? ',' : '');
    });
    */
    result += "\\right]";
    // result.replaceAll("(", "\\left(");
    // result.replaceAll(")", "\\right)");
    result.replaceAll(" ", "");
    return result;
}

function roundTo(n,p=1) {
    return Math.round(n*p)/p;
}

class Vector2 {
    constructor(x=0, y=0) {
        this.x = x;
        this.y = y;
    }

    static copy(v) {
        return new Vector2(v.x, v.y);
    }

    static add(v1, v2) {
        return new Vector2(v1.x+v2.x, v1.y+v2.y);
    }

    static sub(v1, v2) {
        return new Vector2(v1.x-v2.x, v1.y-v2.y);
    }

    add(v) {
        this.x += v.x;
        this.y += v.y;
        return this;
    }

    sub(v) {
        this.x -= v.x;
        this.y -= v.y;
        return this;
    }

    mult(s) {
        this.x *= s;
        this.y *= s;
        return this;
    }

    normalize() {
        this.mult(1/this.magnitude);
        return this;
    }

    get magnitude() {
        return Math.sqrt(this.x**2 + this.y**2);
    }

    set magnitude(mag) {
        this.normalize();
        this.mult(mag);
    }

    moveTo(v) {
        this.x = v.x;
        this.y = v.y;
    }

    toString(d=100) {
        return `(${roundTo(this.x, d)}, ${roundTo(this.y, d)})`;
    }
}

const rad = deg => deg*Math.PI / 180;
const deg = rad => 180*rad / Math.PI;

function polar(...pt) {
    let x,y;
    if (pt.length == 1) {
        x = pt[0].x;
        y = pt[0].y;
    } else {
        x = pt[0];
        y = pt[1];
    }
    let coord = [];
    coord.r = Math.sqrt(x*x + y*y);
    coord.theta = Math.atan(y,x);
    return coord;
}

function car(r,theta) {
    let x,y;
    x = r*Math.cos(theta);
    y = r*Math.sin(theta);
    return [x,y];
}

function getAngles(points, degrees=false) {
    let angs = [];
    for (let i = 0; i < points.length; i++) {
        let pt1 = points[i];
        let pt2 = points[i-1] || new Vector2();

        let angle = polar(Vector2.sub(pt1, pt2)).theta;
        if (degrees) angle = deg(angle.radians);

        angs.push(angle);
    }
    return angs;
}

function getLengths(points) {
    let lengths = [];
    for (let i = 0; i < points.length; i++) {
        let pt1 = points[i];
        let pt2 = points[i-1] || new Vector2();

        let dist = polar(Vector2.sub(pt1, pt2)).r;
        lengths.push(dist);
    }
    return lengths;
}

class arm {
    constructor(...points) {
        this.points = points;
        this.lengths = getLengths(this.points);
        this.angles = getAngles(this.points);
    }

    reach(target, iterations=1) {
        let origin = Vector2.copy(this.points[0]);
        for (let n = 0; n < iterations; n++) {
            this.points[this.points.length-1].moveTo(target);
            for (let i = this.points.length-2; i >= 0; i--) {
                this.points[i].moveTo(Vector2.sub(this.points[i],this.points[i+1]).normalize().mult(this.lengths[i+1]).add(this.points[i+1]));
            }
            this.points[0].moveTo(origin);
            for (let i = 1; i < this.points.length; i++) {
                this.points[i].moveTo(Vector2.sub(this.points[i],this.points[i-1]).normalize().mult(this.lengths[i]).add(this.points[i-1]));
            }
        }

        this.updateAngles();
    }

    updateAngles() {
        this.angles = getAngles(this.points);
    }
}

let segs = []
for (let i = 0; i < 30; i++) {
    segs.push(new Vector2(i, 0));
}
let ikArm = new arm(...segs);

for (let i = 0; i < 100; i++) {
    ikArm.reach(new Vector2(...car(Math.random()*ikArm.points.length, Math.random()*2*Math.PI)), 100);
}
console.group("Kinematic Arm");
ikArm.points.forEach(pt => {
    console.log(pt.toString(1000));
});
console.groupEnd();
console.log(desmosify(ikArm.points, false, "Arm"));