public void draw(PApplet canvas, float scale) {
PVector orient = PVector.fromAngle(orientation);
orient.mult(scale * getRadius());
float x = (float) position.x * scale;
float y = (float) position.y * scale;
float diameter = getRadius() * 2 * scale;
canvas.fill(teamColor);
canvas.stroke(0);
canvas.ellipse(x, y, diameter, diameter);
canvas.line(x, y, x + (float) orient.x, y + (float) orient.y);
// Delegate Decoration to Robot
float heading = orient.heading();
float drawScale = 100f / scale * getRadius();
canvas.translate(x, y);
canvas.rotate(heading);
canvas.scale(drawScale);
// TODO: How to resolve scale, so that teams don't have to mind it also...
decorateRobot(canvas);
canvas.scale(1f / drawScale);
canvas.rotate(-heading);
canvas.translate(-x, -y);
}
// Drawing the box
void display() {
// We look at each body and get its screen position
Vec2 pos = box2d.getBodyPixelCoord(body);
// Get its angle of rotation
float a = body.getAngle();
parent.rectMode(PConstants.CENTER);
parent.pushMatrix();
parent.translate(pos.x, pos.y);
parent.rotate(-a);
parent.fill(127);
parent.stroke(0);
parent.strokeWeight(2);
parent.rect(0, 0, w, h);
parent.popMatrix();
}