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.
 
 
 

99 lines
3.0 KiB

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) {
h /= Manager.SubSteps;
for (let k = 0; k < Manager.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);
// 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 * 0.95 / 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 i = 0; i < this.size; i++){
let p2 = this.X[i].copy();
p2.mult(scale);
let r = Math.sqrt(this.M[i] * 10);
p.ellipse(p2.x, p2.y, r * 2, r * 2);
}
p.pop();
}
}