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
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();
|
|
}
|
|
|
|
} |