Pendulum/public/data/scripts/ts/pendulum.ts

class Pendulum {

    X: Vector[] = [];
    V: Vector[] = [];

    size: number;

    constructor(readonly M: number[], readonly L: number[], readonly color: p5.Color, startAngle: number) {
        console.assert(M.length === L.length, M, L, "Masses and Lengths are not of equal length!");

        this.size = M.length;

        startAngle *= Math.PI / 180;
        let direction = new Vector(Math.sin(startAngle), Math.cos(startAngle));
        let currentPosition = new Vector(0, 0);
        for (let i = 0; i < this.size; i++){
            currentPosition.add(Vector.Mult(direction, L[i]));
            this.X.push(currentPosition.copy());
            this.V.push(new Vector(0, 0));
        }
    }

    // using position based dynamics
    update(h: number) {
        const subSteps = 50;

        h /= subSteps;

        for (let k = 0; k < subSteps; k++) {

            // Classic PBD needs multiple loops
            // Here, I can put all operations safely into one single loop,
            // because the positions and velocities in X, V are sorted
            // from the pendulum's origin to it's end which means
            // that only direct neighbours affect each other

            let previousP = new Vector(0, 0);
            for (let i = 0; i < this.size; i++) {

                // apply external force (gravity)
                this.V[i].addC(0, manager.gravity * h * 50);

                // euler step
                let currentP = Vector.Add(this.X[i],Vector.Mult(this.V[i], h));

                // solve distance constraint
                let w1 = i === 0 ? 0 : 1 / this.M[i - 1];
                let w2 = 1 / this.M[i];

                let s = Vector.Sub(previousP, currentP);
                let n = s.copy();
                n.normalize();
                let l = s.mag();

                let deltaP1 = Vector.Mult(n, -w1 / (w1 + w2) * (l - this.L[i]));
                let deltaP2 = Vector.Mult(n, w2 / (w1 + w2) * (l - this.L[i]));

                previousP.add(deltaP1);
                currentP.add(deltaP2);

                // integrate
                if (i > 0){
                    this.V[i - 1] = Vector.Mult(Vector.Sub(previousP, this.X[i - 1]), 1 / h);
                    this.X[i - 1] = previousP;
                }

                previousP = currentP;
            }

            this.V[this.size - 1] = Vector.Mult(Vector.Sub(previousP, this.X[this.size - 1]), 1 / h);
            this.X[this.size - 1] = previousP;
        }
    }

    draw(){
        p.push();

        p.stroke(this.color);
        p.strokeWeight(1);
        p.fill(255);

        let scale = p.height / Manager.Size;
        let p1 = new Vector(0, 0);
        for (let p2 of this.X){
            p2 = p2.copy();
            p2.mult(scale);
            p.line(p1.x, p1.y, p2.x, p2.y);
            p1 = p2.copy();
        }

        for (let p2 of this.X){
            p2 = p2.copy();
            p2.mult(scale);
            p.ellipse(p2.x, p2.y, 10, 10);
        }

        p.pop();
    }

}