protected void drawRobotWheels(
int column,
int row,
int spacing,
double robotOrientation,
Vector2d robotCenter,
double wheelAngle,
int len) {
int wheelSize = spacing / 5;
Vector2d wheelCenter = new Vector2d(column * spacing, row * spacing);
wheelCenter.rotate(robotOrientation);
wheelCenter.add(robotCenter);
Vector2d posB = new Vector2d(wheelCenter);
Vector2d rot = new Vector2d(len * Math.cos(wheelAngle), len * Math.sin(wheelAngle));
posB.add(rot);
Vector2d guidingLineCenter = new Vector2d(wheelCenter);
rot = new Vector2d(len * Math.cos(robotOrientation), len * Math.sin(robotOrientation));
guidingLineCenter.add(rot);
Vector2d guidingLineLeft = new Vector2d(wheelCenter);
rot =
new Vector2d(
len * Math.cos(robotOrientation + Math.PI / 4),
len * Math.sin(robotOrientation + Math.PI / 4));
guidingLineLeft.add(rot);
Vector2d guidingLineRight = new Vector2d(wheelCenter);
rot =
new Vector2d(
len * Math.cos(robotOrientation - Math.PI / 4),
len * Math.sin(robotOrientation - Math.PI / 4));
guidingLineRight.add(rot);
Graphics2D gx = (Graphics2D) graphics;
// guiding lines
gx.setColor(Color.LIGHT_GRAY);
gx.drawLine(
(int) wheelCenter.x, (int) wheelCenter.y, (int) guidingLineLeft.x, (int) guidingLineLeft.y);
gx.drawLine(
(int) wheelCenter.x,
(int) wheelCenter.y,
(int) guidingLineRight.x,
(int) guidingLineRight.y);
gx.drawLine(
(int) wheelCenter.x,
(int) wheelCenter.y,
(int) guidingLineCenter.x,
(int) guidingLineCenter.y);
// actual line
gx.setStroke(new BasicStroke(2));
gx.setColor(Color.RED);
gx.drawOval(
(int) wheelCenter.x - wheelSize,
(int) wheelCenter.y - wheelSize,
wheelSize * 2,
wheelSize * 2);
gx.drawLine((int) wheelCenter.x, (int) wheelCenter.y, (int) posB.x, (int) posB.y);
gx.setStroke(new BasicStroke());
}
