3D Pendulum tracing a path

pendulum.pde

boolean recording = false;
ArrayList<Pendulum> pendulums;
int frame;
float time;

void setup(){
  size(720, 720, P3D);
  pendulums = new ArrayList<Pendulum>();
  pendulums.add(new Pendulum(0, 0, 0, 150, PI/64, PI/32));
  pendulums.add(new Pendulum(pendulums.get(pendulums.size()-1), 75, PI/64, PI/128));
  pendulums.add(new Pendulum(pendulums.get(pendulums.size()-1), 37.5, PI/256, PI/128));
  time = 0;
  frame = 1;
}

void draw(){
  if (time < TWO_PI || !recording){
    background(28);
    translate(width/2, height/2, 0);
    //rotateX(map(mouseY, 0, height, -PI, PI));
    //rotateY(map(mouseX, 0, width, -PI, PI));
    rotateX(PI/4);
    rotateZ(sin(time)*PI);
    for (int i = 0; i < pendulums.size(); i++){
      pendulums.get(i).update();
      stroke(255, 200);
      fill(255, 100);
      if (i == pendulums.size() - 1) {
        stroke(255);
        strokeWeight(3);
        pendulums.get(i).shape.draw();
        println(pendulums.get(i).shape.vertexes.size());
        fill(255, 200, 200);
      }
      pendulums.get(i).draw();
    }
    time += TWO_PI/513;
    if (recording) save(frame+".gif");
    frame++;
  }
}

class Pendulum{
  float cx, cy, cz, px, py, pz, size;
  float theta, phi, tc, pc;
  Pendulum parent;
  Shape shape;
  Pendulum(float xa, float ya, float za, float sizea, float tca, float pca){
    cx = xa;
    cy = ya;
    cz = za;
    size = sizea;
    tc = tca;
    pc = pca;
    shape = new Shape();
  }

  Pendulum(Pendulum parenta, float sizea, float tca, float pca){
    size = sizea;
    tc = tca;
    pc = pca;
    shape = new Shape();
    parent = parenta;
    cx = parent.px;
    cy = parent.py;
    cz = parent.pz;
    theta = parent.theta;
    phi = parent.phi;
  }

  void update(){
    px = size * sin(theta) * cos(phi);
    py = size * sin(theta) * sin(phi);
    pz = size * cos(theta);
    theta += tc;
    phi += pc;
    if (parent != null) {
      px = size * sin(theta+parent.theta) * cos(phi+parent.phi);
      py = size * sin(theta+parent.theta) * sin(phi+parent.phi);
      pz = size * cos(theta+parent.theta);
      cx = parent.px;
      cy = parent.py;
      cz = parent.pz;
    }
    if (parent != null){
      shape.addPoint(new PVector(parent.cx+px+cx, parent.cy+py+cy, parent.cz+pz+cz));
    } else {
      shape.addPoint(new PVector(cx+px, cy+py, cz+pz));
    }
    theta = theta % TWO_PI;
    phi = phi % TWO_PI;
  }

  void draw(){
    pushMatrix();
    if (parent == null) {
      translate(cx, cy, cz);
    } else {
      translate(parent.px+parent.cx, parent.py+parent.cy, parent.pz+parent.cz);
    }
    strokeWeight(1);
    line(0, 0, 0, px, py, pz);
    noStroke();
    translate(px, py, pz);
    sphere(5);
    popMatrix();
  }
}

class Shape{
  ArrayList<PVector> vertexes;
  Shape(){
    vertexes = new ArrayList<PVector>();
  }

  void addPoint(PVector p){
    vertexes.add(p);
  }

  void draw(){
    beginShape();
    noFill();
    stroke(255, 200, 200);
    for(PVector p: vertexes){
      vertex(p.x, p.y, p.z);
    }
    endShape();
  }
}