二重振り子

float g=9.8;
float m1=10;
float m2=1;
float m12=m1+m2;
float L1=100;
float L2=100;
float theta1=PI-1.0e2;
float theta2=PI+1.0e2;
float dtheta1=0;
float dtheta2=0;
float dt=0.1;

void  setup(){
  size(400,400);
  frameRate(60);
}

void draw(){
  background(0);
  float S=sin(theta1-theta2);
  float C=cos(theta1-theta2);

  float A=L1*(m1+m2*sq(S));
  float ddtheta1=(-m1*g*sin(theta1)-m2*(g*sin(theta1)+L2*sq(dtheta2)*S+C*(L1*sq(dtheta1)*S-g*sin(theta2))))/A;
  float ddtheta2=(m12*(L1*sq(dtheta1)*S-g*sin(theta2)+g*sin(theta1)*C)+m2*L2*sq(dtheta2)*C*S)/A;
  
  dtheta1+=ddtheta1*dt;
  dtheta2+=ddtheta2*dt;
  theta1+=dtheta1*dt;
  theta2+=dtheta2*dt;
  
  stroke(255);
  strokeWeight(5);
  translate(width/2,height/2);
  float x1=L1*sin(theta1);
  float y1=L1*cos(theta1);
  float x2=x1+L2*sin(theta2);
  float y2=y1+L2*cos(theta2);
  line(0,0,x1,y1);
  line(x1,y1,x2,y2);
}