歩行ロボットの実験を通して周期関数としての三角関数の使い方を学ぶ
最近、GLUTによる「手抜き」OpenGL入門
その中で歩行ロボットの実験
問題のヒントとして腕や足の振りを周期的に変化させて実装しなさいとあったのですが数学が大の苦手で初めは何のことかさっぱり分からず、さらにどこにも答えや解答例が載ってなかったので苦労しました。
関数 display() 内では，これらの変数を armleg() の引数 r1, r2 に与えています．この単位は度で指定します． これらを時間の進行に伴って周期的に変化させれば， 腕や足を振ることができます． 代表的な周期関数には三角関数 (sin, cos) があります．
周期関数をWikiで調べて数字を色々といじって何とか作成したプログラムが↓です。もっとスマートなやり方があるという方がいれば遠慮なく突っ込んでやってくださいw
クラス図は以下のようになります。
まずはメインクラスの実装です。FPSの制御はThread#sleepメソッドを利用しています。もっと正確なFPS制御をしたい場合は正確なFPS
import java.awt.Frame; import java.awt.event.WindowAdapter; import java.awt.event.WindowEvent; import javax.media.opengl.GL; import javax.media.opengl.GLAutoDrawable; import javax.media.opengl.GLEventListener; import javax.media.opengl.GLCanvas; import javax.media.opengl.glu.GLU; import com.sun.opengl.util.Animator; public class Main implements GLEventListener { // 光源の設定 private float LIGHT_POSITION[] = { 3.0f, 4.0f, 5.0f, 1.0f }; private Animator animator; private Ground ground = new Ground(); private Robot robot = new Robot(); public Main(){ Frame frame = new Frame("テスト"); GLCanvas canvas = new GLCanvas(); canvas.addGLEventListener(this); frame.add(canvas); frame.setSize(300, 300); animator = new Animator(canvas); animator.start(); frame.addWindowListener(new WindowAdapter() { public void windowClosing(WindowEvent e) { animator.stop(); System.exit(0); } }); frame.setVisible(true); } public static void main(String[] args) { new Main2(); } public void init(GLAutoDrawable drawable){ GL gl = drawable.getGL(); gl.glClearColor(1.0f, 1.0f, 1.0f, 1.0f); gl.glEnable(GL.GL_DEPTH_TEST); gl.glEnable(GL.GL_CULL_FACE); gl.glEnable(GL.GL_LIGHTING); gl.glEnable(GL.GL_LIGHT0); gl.glEnable(GL.GL_NORMALIZE); } public void display(GLAutoDrawable drawable){ GL gl = drawable.getGL(); gl.glClear(GL.GL_COLOR_BUFFER_BIT | GL.GL_DEPTH_BUFFER_BIT); gl.glLoadIdentity(); gl.glTranslatef(0.0f, 0.0f, -10.0f); gl.glLightfv(GL.GL_LIGHT0, GL.GL_POSITION, LIGHT_POSITION, 0); ground.display(drawable); robot.display(drawable); // FPSの制御 60fpsで描画(1/60秒=16ミリ秒) try { Thread.sleep(16); } catch (InterruptedException e) { e.printStackTrace(); } } public void displayChanged(GLAutoDrawable drawable, boolean modeChanged, boolean deviceChanged){ } public void reshape(GLAutoDrawable drawable, int x, int y, int w, int h){ GL gl = drawable.getGL(); /* ウィンドウ全体をビューポートにする */ gl.glViewport(0, 0, w, h); /* 透視変換行列の指定 */ gl.glMatrixMode(GL.GL_PROJECTION); /* 透視変換行列の初期化 */ gl.glLoadIdentity(); GLU glu = new GLU(); glu.gluPerspective(30.0, (double)w / (double)h, 1.0, 100.0); /* モデルビュー変換行列の指定 */ gl.glMatrixMode(GL.GL_MODELVIEW); } }
地面描画用クラスの実装です。
public class Ground { private static final float GROUND_COLOR[][] = { { 0.3f, 0.3f, 0.3f, 1.0f }, { 0.6f, 0.6f, 0.6f, 1.0f } }; public void display(GLAutoDrawable drawable) { GL gl = drawable.getGL(); gl.glBegin(GL.GL_QUADS); gl.glNormal3d(0.0, 1.0, 0.0); int len = 1; int sourcePoint = -5; float height = -1.5f;; for (int i = sourcePoint; i <= -sourcePoint; ++i) { for (int j = sourcePoint; j < -sourcePoint; ++j) { gl.glMaterialfv(GL.GL_FRONT, GL.GL_DIFFUSE, GROUND_COLOR[Math.abs((j+i) % 2)], 0); gl.glVertex3d(j, height, i); gl.glVertex3d(j, height, i + len); gl.glVertex3d(j + len, height, i + len); gl.glVertex3d(j + len, height, i); } } gl.glEnd(); } }
ロボットの基本パーツ(頭や体)を描画するクラスの実装です。
public interface ShapeDrawable { void setPosition(double x, double y, double z); void display(GLAutoDrawable drawable); } public class BodyPart implements ShapeDrawable { private static final float RED[] = { 0.8f, 0.2f, 0.2f, 1.0f }; private static final int FACE[][] = { { 0, 1, 2, 3 }, { 1, 5, 6, 2 }, { 5, 4, 7, 6 }, { 4, 0, 3, 7 }, { 4, 5, 1, 0 }, { 3, 2, 6, 7 } }; private static final double NORMAL[][] = { { 0.0, 0.0,-1.0 }, { 1.0, 0.0, 0.0 }, { 0.0, 0.0, 1.0 }, {-1.0, 0.0, 0.0 }, { 0.0,-1.0, 0.0 }, { 0.0, 1.0, 0.0 } }; private double width, height, depth; protected double x, y, z; public BodyPart(double width, double height, double depth) { this.width = width; this.height = height; this.depth = depth; } protected double getWidth() { return width; } protected double getHeight() { return height; } protected double getDepth() { return depth; } public void setPosition(double x, double y, double z) { this.x = x; this.y = y; this.z = z; } public void display(GLAutoDrawable drawable) { GL gl = drawable.getGL(); gl.glPushMatrix(); translatedPosition(gl); drawBox(gl); gl.glPopMatrix(); } protected final void drawBox(GL gl) { double hx = width * 0.5, hz = depth * 0.5; double vertex[][] = { { -hx, -height, -hz }, { hx, -height, -hz }, { hx, 0.0, -hz }, { -hx, 0.0, -hz }, { -hx, -height, hz }, { hx, -height, hz }, { hx, 0.0, hz }, { -hx, 0.0, hz } }; /* 材質を設定する */ gl.glMaterialfv(GL.GL_FRONT, GL.GL_DIFFUSE, RED, 0); gl.glBegin(GL.GL_QUADS); for (int i = 0; i < 6; ++i) { gl.glNormal3dv(NORMAL[i], 0); for (int j = 4; --j >= 0;) { gl.glVertex3dv(vertex[FACE[i][j]], 0); } } gl.glEnd(); } protected void translatedPosition(GL gl) { gl.glTranslated(x, y, z); } }
ロボットの腕と足を描画するクラスの実装です。ポイントは腕や足のふり幅を三角関数の周期性を利用して計算しているところです。
public abstract class Limb extends BodyPart { /* 上肢の最大角度 */ private static final double MAX_ANGLE_OF_UPPER_JOINT = 25; /* 上肢の角度 */ private double upperJointAngle; /* 下肢の角度 */ private double lowerJointAngle; /* 前に進むか後ろに進むか */ private int forwardFactor = 1; private int cycle; public Limb(double girth, double length, boolean forward) { super(girth, length, girth); if (!forward) { forwardFactor *= -1; } } @Override public void display(GLAutoDrawable drawable) { GL gl = drawable.getGL(); gl.glPushMatrix(); // 上肢を描画 translatedPosition(gl); gl.glRotated(upperJointAngle, 1.0, 0.0, 0.0); drawBox(gl); // 下肢を描画 gl.glTranslated(0.0, -0.05 - getHeight(), 0.0); gl.glRotated(lowerJointAngle, 1.0, 0.0, 0.0); drawBox(gl); // 角度の計算 cycle %= 360; double rad = Math.toRadians(cycle); // 三角関数の周期性を利用して-25〜+25度の範囲で傾きを計算 upperJointAngle = MAX_ANGLE_OF_UPPER_JOINT * Math.sin(rad) * forwardFactor; lowerJointAngle = calcLowerJointAngle(cycle, forwardFactor); cycle += 5; gl.glPopMatrix(); } protected abstract double calcLowerJointAngle(int cycle, int forwardFactor); } public class Arm extends Limb { private static final double MAX_ANGLE_OF_LOWER_JOINT = 20; public Arm(double girth, double length, boolean forward) { super(girth, length, forward); } @Override protected double calcLowerJointAngle(int cycle, int forwardFactor) { double rad = Math.toRadians(cycle); return MAX_ANGLE_OF_LOWER_JOINT * (Math.cos(rad) - forwardFactor) * -forwardFactor; } } public class Leg extends Limb { private static final double MAX_ANGLE_OF_LOWER_JOINT = 15; public Leg(double girth, double length, boolean forward) { super(girth, length, forward); } @Override protected double calcLowerJointAngle(int cycle, int forwardFactor) { double rad = Math.toRadians(cycle); return MAX_ANGLE_OF_LOWER_JOINT * (Math.cos(rad) + forwardFactor) * -forwardFactor; } }
最後ににロボット描画用クラスの実装です。
public class Robot { private ShapeDrawable head = new BodyPart(0.20, 0.25, 0.22); private ShapeDrawable body = new BodyPart(0.4, 0.6, 0.3); private ShapeDrawable rightArm = new Arm(0.16, 0.4, true); private ShapeDrawable leftArm = new Arm(0.16, 0.4, false); private ShapeDrawable rightLeg = new Leg(0.2, 0.4, false); private ShapeDrawable leftLeg = new Leg(0.2, 0.4, true); private double angle; private int cycle; public void display(GLAutoDrawable drawable) { double theta = Math.toRadians(angle); double robotX = Math.sin(theta) * 2; double robotY = Math.cos(theta) * 2; double rad = Math.cos(Math.toRadians(cycle)); if (rad < 0) {rad *= -1;} cycle += 5; GL gl = drawable.getGL(); gl.glPushMatrix(); // ロボットを移動させる gl.glTranslated(robotX, 0.0, robotY); gl.glRotated(angle + 90, 0.0, 1.0, 0.0); // 頭 head.setPosition(0.0, 0.3, 0.0); head.display(drawable); // 胴体 body.display(drawable); //右腕 rightArm.setPosition(-0.28, 0.0, 0.0); rightArm.display(drawable); // 左腕 leftArm.setPosition(0.28, 0.0, 0.0); leftArm.display(drawable); // 右足 rightLeg.setPosition(-0.1, -0.65, 0.0); rightLeg.display(drawable); // 左足 leftLeg.setPosition(0.1, -0.65, 0.0); leftLeg.display(drawable); gl.glPopMatrix(); angle -= rad; if (angle >= 360.0f) { angle = 0.0f; } } }
