You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

94 lines
2.7 KiB

class Pendulum {
2 years ago
X: Vector[] = [];
V: Vector[] = [];
2 years ago
size: number;
2 years ago
constructor(readonly M: number[], readonly L: number[], readonly color: p5.Color) {
console.assert(M.length === L.length, M, L, "Masses and Lengths are not of equal length!");
this.size = M.length;
let currentPosition = new Vector(0, 0);
for (let i = 0; i < this.size; i++){
let a = Math.sqrt(L[i] * L[i] / 2);
currentPosition.addC(-L[i], 0);
this.X.push(currentPosition.copy());
this.V.push(new Vector(0, 0));
}
2 years ago
}
// using position based dynamics
update(h: number) {
const subSteps = 50;
2 years ago
h /= subSteps;
2 years ago
for (let k = 0; k < subSteps; k++) {
2 years ago
// 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
2 years ago
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);
2 years ago
// euler step
let currentP = Vector.Add(this.X[i],Vector.Mult(this.V[i], h));
2 years ago
// solve distance constraint
let w1 = i === 0 ? 0 : 1 / this.M[i - 1];
let w2 = 1 / this.M[i];
2 years ago
let s = Vector.Sub(previousP, currentP);
let n = s.copy();
n.normalize();
let l = s.mag();
2 years ago
let deltaP1 = Vector.Mult(n, -w1 / (w1 + w2) * (l - this.L[i]));
let deltaP2 = Vector.Mult(n, w2 / (w1 + w2) * (l - this.L[i]));
2 years ago
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;
}
2 years ago
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;
}
2 years ago
}
draw(){
p.push();
2 years ago
p.stroke(this.color);
p.strokeWeight(1);
p.fill(255);
let p1 = new Vector(0, 0);
for (let p2 of this.X){
p.line(p1.x, p1.y, p2.x, p2.y);
p1 = p2.copy();
}
2 years ago
for (let p2 of this.X){
p.ellipse(p2.x, p2.y, 10, 10);
}
p.pop();
2 years ago
}
}