OpenGL ESでクォータニオン

というわけで回転できた。
割と疲れた。

はじめPapervision3Dのを参考にして作ってみたけどなんか回転がおかしいのと、
うまくクォータニオンから回転行列が作れないので他のやり方を探した。
そこで参考にしたサイトがここ。

途中経過は色々面倒だけど結局、

float sinX = (float)Math.sin(ax/2);
float cosX = (float)Math.cos(ax/2);
float sinY = (float)Math.sin(ay/2);
float cosY = (float)Math.cos(ay/2);
float sinZ = (float)Math.sin(-az/2);
float cosZ = (float)Math.cos(-az/2);
float a = sinY * sinX;
float b = cosY * cosX;
float c = sinY * cosX;
float d = cosY * sinX;

Matrix3D m = new Matrix3D();

m.set(11, cosZ * d - sinZ * c);
m.set(12, sinZ * d + cosZ * c);
m.set(13, sinZ * b - cosZ * a);
m.set(14, cosZ * b + sinZ * a);

でクォータニオンが得られるので楽。
z軸回転だけ回転角を反転してるのはOpenGLの座標系と合わせるため。
Papervision3Dの座標系がいまいちよくわからないのは、
ここら辺がおかしいからかもしれない。

以下動作画面とソース。
http://screencast.com/t/WTVfq3qW9

ソース

DisplayObject3D.java

package net.swelt.android.opengltest;

import java.nio.ByteBuffer;
import java.nio.IntBuffer;

import javax.microedition.khronos.opengles.GL10;

public class DisplayObject3D {
	protected IntBuffer mVertexBuffer;
	protected IntBuffer mColorBuffer;
	protected IntBuffer mTransformBuffer;
	protected ByteBuffer mIndexBuffer;
	private int x = 0;
	private int y = 0;
	private int z = 0;
	private boolean visible = true;
	private boolean picking = true;
	private String name = "";
	protected float scaleX = 1.0f;
	protected float scaleY = 1.0f;
	protected float scaleZ = 1.0f;	
	protected Matrix3D transform = new Matrix3D();
	protected boolean transformDirty = true;
	private float rotationX = 0.0f;
	private float rotationY = 0.0f;
	private float rotationZ = 0.0f;
	
	public void update() {
		if( transformDirty ) updateTransform();
	}
	
	public void draw(GL10 gl) {
		//if( transformDirty ) updateTransform();
		
		//gl.glVertexPointer(3, GL10.GL_FIXED, 0, mVertexBuffer);
		gl.glVertexPointer(3, GL10.GL_FIXED, 0, mTransformBuffer);
		gl.glColorPointer(4, GL10.GL_FIXED, 0, mColorBuffer);
		gl.glDrawElements(GL10.GL_TRIANGLES, mIndexBuffer.capacity(), GL10.GL_UNSIGNED_BYTE, mIndexBuffer);
	}
	
	public void reset() {
		x = 0;
		y = 0;
		z = 0;
		scaleX = 1.0f;
		scaleY = 1.0f;
		scaleZ = 1.0f;
		rotationX = 0.0f;
		rotationY = 0.0f;
		rotationZ = 0.0f;
		
		transformDirty = true;
	}
	
	public void move(int x, int y, int z) {
		this.x = x;
		this.y = y;
		this.z = z;
		
		transformDirty = true;
	}
	
	public void translate(int x, int y, int z) {
		this.x += x;
		this.y += y;
		this.z += z;
		
		transformDirty = true;
	}
	
	public int getX() { return x; };
	public int getY() { return y; };
	public int getZ() { return z; };

	public void setX(int x) {
		this.x = x;
		
		transformDirty = true;
	}
	
	public void setY(int y) {
		this.y = y;
		
		transformDirty = true;
	}
	
	public void setZ(int z) {
		this.z = z;
		
		transformDirty = true;
	}
	
	//-----rotation
	public float getRotaionX() { return rotationX; };
	public float getRotaionY() { return rotationY; };
	public float getRotaionZ() { return rotationZ; };
	
	public void rotate(float ax, float ay, float az) {
		rotationX += ax;
		rotationY += ay;
		rotationZ += az;
		
		transformDirty = true;
	}
	
	public void setRotationX(float r) {
		rotationX = r;
		
		transformDirty = true;
	}
	
	public void setRotationY(float r) {
		rotationY = r;
		
		transformDirty = true;
	}
	
	public void setRotationZ(float r) {
		rotationZ = r;
		
		transformDirty = true;
	}
	
	
	//-----scale
	public float getScale() { return scaleX; };
	public float getScaleX() { return scaleX; };
	public float getScaleY() { return scaleY; };
	public float getScaleZ() { return scaleZ; };
	
	public void setScale(float scale) {
		scaleX = scaleY = scaleZ = scale;
		
		transformDirty = true;
	}
	
	//-----visibility
	public boolean getVisibility() { return visible; };	
	public void setVisibility(boolean visible) {
		this.visible = visible;
	}
	
	//-----picking
	public boolean getPicking() { return picking; };	
	public void setPicking(boolean picking) {
		this.picking = picking;
	}
	
	//-----name
	public String getName() { return name; };
	public void setName(String name) {
		this.name = name;
	}
		
	//-----transform
	public void updateTransform() {
		Matrix3D m = Matrix3D.eular2Quaternion(rotationX, rotationY, rotationZ);
		m = Matrix3D.quaternion2Euler(m.get(11), m.get(12), m.get(13), m.get(14));
		m.set(44, 1.0f);
				
		Matrix3D transform = new Matrix3D(Matrix3D.IDENTITY);		
		transform.set(41, x);
		transform.set(42, y);
		transform.set(43, z);
		transform.calc(m, transform);
		
		Matrix3D scaleM = new Matrix3D(Matrix3D.IDENTITY);
		scaleM.set(11, scaleX);
		scaleM.set(22, scaleY);
		scaleM.set(33, scaleZ);
		scaleM.set(44, 1.0f);
		
		this.transform.calc(transform, scaleM);
		
		Matrix3D vertexM = new Matrix3D();
		Matrix3D r = new Matrix3D();
		vertexM.set(14, 1.0f);
		for( int i = 0; i < mVertexBuffer.capacity(); i += 3 ) {
			vertexM.set(11, (float)mVertexBuffer.get(i));
			vertexM.set(12, (float)mVertexBuffer.get(i+1));
			vertexM.set(13, (float)mVertexBuffer.get(i+2));
						
			r.calc(vertexM, this.transform);
			
			mTransformBuffer.put(i, (int)r.get(11));
			mTransformBuffer.put(i+1, (int)r.get(12));
			mTransformBuffer.put(i+2, (int)r.get(13));;
		}
		
		transformDirty = false;
	}
	
	//-----string
	public String toString() {
		String s;
		
		s = "x:" + x + " y:" + y + " z:" + z + "\n";
		s += "scaleX:" + scaleX + "\nscaleY:" + scaleY + "\nscaleZ:" + scaleZ + "\n";
		s += "rotationX:" + rotationX + "\nrotationY:" + rotationY + "\nrotationZ:" + rotationZ + "\n";
		
		return s;
		
	}
}

Matrix3D.java

package net.swelt.android.opengltest;

public class Matrix3D {
	public static float[] IDENTITY = {1.0f, 0, 0, 0,
									  0, 1.0f, 0, 0,
									  0, 0, 1.0f, 0,
									  0, 0, 0, 1.0f};
	
	private float n11 = 0;
	private float n12 = 0;
	private float n13 = 0;
	private float n14 = 0;
	private float n21 = 0;
	private float n22 = 0;
	private float n23 = 0;
	private float n24 = 0;
	private float n31 = 0;
	private float n32 = 0;
	private float n33 = 0;
	private float n34 = 0;
	private float n41 = 0;
	private float n42 = 0;
	private float n43 = 0;
	private float n44 = 0;
	
	public Matrix3D() {		
	}
	
	public Matrix3D(float[] ar) {
		set(ar);
	}
	
	public float get(int index) {
		switch( index ) {
		case 11: return n11;
		case 12: return n12;
		case 13: return n13;
		case 14: return n14;
		case 21: return n21;
		case 22: return n22;
		case 23: return n23;
		case 24: return n24;
		case 31: return n31;
		case 32: return n32;
		case 33: return n33;
		case 34: return n34;
		case 41: return n41;
		case 42: return n42;
		case 43: return n43;
		case 44: return n44;
		}
		return 0;
	}
	
	public void set(int index, float n) {
		switch( index ) {
		case 11: n11 = n; break;
		case 12: n12 = n; break;
		case 13: n13 = n; break;
		case 14: n14 = n; break;
		case 21: n21 = n; break;
		case 22: n22 = n; break;
		case 23: n23 = n; break;
		case 24: n24 = n; break;
		case 31: n31 = n; break;
		case 32: n32 = n; break;
		case 33: n33 = n; break;
		case 34: n34 = n; break;
		case 41: n41 = n; break;
		case 42: n42 = n; break;
		case 43: n43 = n; break;
		case 44: n44 = n; break;
		}		
	}
	
	public void set(float[] ar) {
		if( ar.length >= 12 ) {
			n11 = ar[0]; n12 = ar[1]; n13 = ar[2]; n14 = ar[3];
			n21 = ar[4]; n22 = ar[5]; n23 = ar[6]; n24 = ar[7];
			n31 = ar[8]; n32 = ar[9]; n33 = ar[10]; n34 = ar[11];
			n41 = ar[12]; n42 = ar[13]; n43 = ar[14]; n44 = ar[15];
		}		
	}
	
	public float[] get() {
		return new float[] {n11, n12, n13, n14, n21, n22, n23, n24, n31, n32, n33, n34, n41, n42, n43, n44};
	}

	public void calc(Matrix3D a, Matrix3D b) {
		float[] m1 = a.get();
		float[] m2 = b.get();
		float[] r = new float[m1.length];
		
		for( int j = 0; j < 4; ++ j ) {
			for( int i = 0; i < 4; ++ i ) {
				int n = 4*j;
				r[n+i] = m1[n]*m2[i]+m1[n+1]*m2[4+i]+m1[n+2]*m2[8+i]+m1[n+3]*m2[12+i];
			}
		}
		
		set(r);
	}
	
	//-----rotation
	public static Matrix3D euler2Quaternion(float ax, float ay, float az) {		
		float sinX = (float)Math.sin(ax/2);
		float cosX = (float)Math.cos(ax/2);
		float sinY = (float)Math.sin(ay/2);
		float cosY = (float)Math.cos(ay/2);
		float sinZ = (float)Math.sin(-az/2);
		float cosZ = (float)Math.cos(-az/2);
		float a = sinY * sinX;
		float b = cosY * cosX;
		float c = sinY * cosX;
		float d = cosY * sinX;
		
		Matrix3D m = new Matrix3D();
		
		m.set(11, cosZ * d - sinZ * c);
		m.set(12, sinZ * d + cosZ * c);
		m.set(13, sinZ * b - cosZ * a);
		m.set(14, cosZ * b + sinZ * a);
		
		return m;
	}
	
	public static Matrix3D quaternion2Euler(float x, float y, float z, float w) {
		float xx = x * x;
		float xy = x * y;
		float xz = x * z;
		float xw = x * w;
		
		float yy = y * y;
		float yz = y * z;
		float yw = y * w;
		
		float zz = z * z;
		float zw = z * w;
		
		Matrix3D m = new Matrix3D();
		
		m.set(11, 1 - 2 * (yy + zz));
		m.set(12, 2 * (xy - zw));
		m.set(13, 2 * (xz + yw));
		m.set(21, 2 * (xy + zw));
		m.set(22, 1 - 2 * (xx + zz));
		m.set(23, 2 * (yz - xw));
		m.set(31, 2 * (xz - yw));
		m.set(32, 2 * (yz + xw));
		m.set(33, 1 - 2 * (xx + yy));

		for( int i = 1; i < 4; ++ i ) {
			for( int j = 1; j < 4; ++ j ) {
				float n = m.get(i * 10 + j);
				if( Math.abs(n) < 1.0e-6 ) m.set(i * 10 + j, 0.0f);
			}
		}
		
		return m;
	}
	
	//-----string
	public String toString() {
		String s;
		
		s = "" + n11 + "," + n12 + "," + n13 + "," + n14 + "\n";
		s += "" + n21 + "," + n22 + "," + n23 + "," + n24 + "\n";
		s += "" + n31 + "," + n32 + "," + n33 + "," + n34 + "\n";
		s += "" + n41 + "," + n42 + "," + n43 + "," + n44 + "\n";
		
		return s;
	}
}