class DoublePendulum {
float m1, m2; //point masses at the ends of the arms
float g; //gravity constant
float l1, l2; //arm lengths
float[] x = new float[4]; //state vector, state vars are theta1 theta1' theta2 theta2'
float[] dx = new float[4]; //for storing the value of the state function at the current state
void update(float h) {
//evaluate the state functions
dx[0] = x[1];
dx[1] = -g * (2 * m1 + m2) * sin(x[0]) -
m2 * g * sin(x[0] - 2 * x[2]) -
2 * sin(x[0] - x[2]) * m2 * (x[3] * x[3] * l2 +
x[1] * x[1] * l1 * cos(x[0] - x[2]));
dx[1] = dx[1] / (l1 * (2 * m1 + m2 - m2 * cos(2 * x[0] - 2 * x[2])));
dx[1] = dx[1] - 0.08 * x[1] / (abs(x[1]) + 0.01); //friction, should probably be elaborated on
dx[2] = x[3];
dx[3] = 2 * sin(x[0] - x[2]) * (x[1] * x[1] * l1 * (m1 + m2) +
g * (m1 + m2) * cos(x[0]) +
x[3] * x[3] * l2 * m2 * cos(x[0] - x[2]));
dx[3] = dx[3] / (l2 * (2 * m1 + m2 - m2 * cos(2 * x[0] - 2 * x[2])));
dx[3] = dx[3] - 0.1 * x[3] / (abs(x[3]) + 0.01); //friction
//euler step
x[0] += h * dx[0];
x[1] += h * dx[1];
x[2] += h * dx[2];
x[3] += h * dx[3];
}
void draw() {
float endx = centerx + s * l1 * sin(x[0]);
float endy = centery + s * l1 * cos(x[0]);
line(centerx, centery, endx, endy);
line(endx, endy,
endx + s * l2 * sin(x[2]),
endy + s * l2 * cos(x[2]));
}
}
DoublePendulum pend = new DoublePendulum();
PGraphics bg; //the trace will be drawn into this
float posx, posy, newposx, newposy; //these keep track of the tip's position
float centerx, centery; //anchor point of the pendulum
int w;
int h;
float s = 30.; //scaling factor for drawing the arms
void setup() {
size(400, 400);
w = 400;
h = 400;
bg = createGraphics(w, h);
centerx = w / 2.;
centery = h / 2.;
pend.m1 = random(1, 3);
pend.m2 = random(0.5, 2);
pend.l1 = random(0.8, 4);
pend.l2 = 5 - pend.l1;
pend.g = 10;
pend.x[0] = 3;
pend.x[1] = 0;
pend.x[2] = 3.14;
pend.x[3] = 0;
posx = centerx + s * pend.l1 * sin(pend.x[0]) + s * pend.l2 * sin(pend.x[2]);
posy = centery + s * pend.l1 * cos(pend.x[0]) + s * pend.l2 * cos(pend.x[2]);
background(0);
smooth();
}
void draw() {
background(255);
pend.update(0.01);
newposx = centerx + s * pend.l1 * sin(pend.x[0]) + s * pend.l2 * sin(pend.x[2]);
newposy = centery + s * pend.l1 * cos(pend.x[0]) + s * pend.l2 * cos(pend.x[2]);
bg.beginDraw();
bg.smooth();
//might look better without smooting as the individual segments don't play nicely with eachother
bg.colorMode(HSB, 360, 100, 100);
bg.noStroke();
if (frameCount % 20 == 0) {
bg.fill(0, 0.1);
bg.rect(0, 0, w, h);
}
float vel = sqrt((posx - newposx) * (posx - newposx) + (posy - newposy) * (posy - newposy));
// should calculate vel wrt the actual states instead of the xy pos as it doesn't scale well right now
bg.stroke(frameCount / 5 % 360, 100 - vel * 15, vel * 20 + 50, vel * 10 + 50);
bg.strokeWeight(5);
bg.line(posx, posy, newposx, newposy);
bg.stroke(frameCount / 5 % 360, 100 - vel * 20, vel * 20 + 50, vel * 20 + 100);
bg.strokeWeight(2);
bg.line(posx, posy, newposx, newposy);
bg.endDraw();
posx = newposx;
posy = newposy;
image(bg, 0, 0);
stroke(100, 100, 100);
strokeWeight(2);
pend.draw();
}
void mouseClicked() {
this.setup();
}