openGL es2.0 创建物理世界_颜色立方体刚体_颜色平面刚体以及四元数转换

public void initWorld(){
CollisionConfiguration collisionConfiguration=new DefaultCollisionConfiguration();
CollisionDispatcher dispatcher=new CollisionDispatcher(collisionConfiguration);
Vector3f worldAabbMin=new Vector3f(-10000, -10000, -10000);
Vector3f worldAabbMax=new Vector3f(10000, 10000, 10000);
int maxProxies=1024;
AxisSweep3 overlappingPairCache=new AxisSweep3(worldAabbMin, worldAabbMax, maxProxies);
SequentialImpulseConstraintSolver solver=new SequentialImpulseConstraintSolver();
dynamicsWorld=new DiscreteDynamicsWorld(dispatcher, overlappingPairCache, solver, collisionConfiguration);
dynamicsWorld.setGravity(new Vector3f(0, -2f, 0));
boxShape=new BoxShape(new Vector3f(CUBE_HALFSIZE, CUBE_HALFSIZE, CUBE_HALFSIZE));
plainShape=new StaticPlaneShape(new Vector3f(0, 1, 0), 0f);
dw_CubeColor=new DW_CubeColor(CUBE_HALFSIZE, 1, 0, 2, 0, boxShape, dynamicsWorld, ShaderManager.getOnlyColorShaderProgram());
dw_RectColor=new DW_RectColor(5, 0f, plainShape, dynamicsWorld, ShaderManager.getOnlyColorShaderProgram());
}

package com.gzdxid.physicalworld;

import javax.vecmath.Quat4f;
import javax.vecmath.Vector3f;

import android.opengl.GLES20;

import com.bulletphysics.collision.shapes.CollisionShape;
import com.bulletphysics.dynamics.DiscreteDynamicsWorld;
import com.bulletphysics.dynamics.RigidBody;
import com.bulletphysics.linearmath.DefaultMotionState;
import com.bulletphysics.linearmath.Transform;
import com.gzdxid.utils.DrawRectColor;
import com.gzdxid.utils.MatrixState;
import com.gzdxid.utils.QuatUtil;

//Draw World Object
public class DW_CubeColor {

private DrawRectColor drawRectColor;
private RigidBody rigidBody;
private float Offset;

public DW_CubeColor(float halfSize, float mass, float sx, float sy, float sz, CollisionShape collisionShape, DiscreteDynamicsWorld dynamicsWorlds, int mProgram) {
initRect(halfSize, mProgram);
initPropertity(mass, sx, sy, sz, collisionShape, dynamicsWorlds);
}

private void initRect(float halfSize, int mProgram) {
Offset = halfSize/2;
drawRectColor = new DrawRectColor(halfSize, halfSize, mProgram);
}

private void initPropertity(float mass, float sx, float sy, float sz, CollisionShape collisionShape, DiscreteDynamicsWorld dynamicsWorlds) {
// TODO Auto-generated method stub
boolean isDynamic = (mass != 0);
Vector3f localInertia = new Vector3f(0, 0, 0);
if (isDynamic) {
collisionShape.calculateLocalInertia(mass, localInertia);
}
Transform starTransform = new Transform();
starTransform.setIdentity();
starTransform.origin.set(new Vector3f(sx, sy, sz));
DefaultMotionState myMotionState = new DefaultMotionState(starTransform);
rigidBody = new RigidBody(mass, myMotionState, collisionShape, localInertia);
dynamicsWorlds.addRigidBody(rigidBody);
rigidBody.setRestitution(0.6f);
rigidBody.setFriction(0.8f);
}

public void drawSelf(float r, float g, float b, float a) {
MatrixState.pushMatrix();
MatrixState.rotate(-180, 1, 0, 0);
MatrixState.translate(0, 0, 0);
Transform transform = rigidBody.getMotionState().getWorldTransform(new Transform());
MatrixState.translate(transform.origin.x, transform.origin.y, transform.origin.z);
Quat4f rotateQuat4f = transform.getRotation(new Quat4f());
if (rotateQuat4f.x != 0 || rotateQuat4f.y != 0 || rotateQuat4f.z != 0) {
float[] fa = QuatUtil.fromQUATtoAXYZ(rotateQuat4f);
MatrixState.rotate(fa[0], fa[1], fa[2], fa[3]);
}
drawCube(r, g, b, a);
MatrixState.popMatrix();
}

private void drawCube(float r, float g, float b, float a) {
GLES20.glEnable(GLES20.GL_CULL_FACE);
GLES20.glEnable(GLES20.GL_DEPTH_TEST);
MatrixState.pushMatrix();// 保护现场
MatrixState.translate(0, Offset, 0);// 执行平移
MatrixState.rotate(-90, 1, 0, 0);// 执行旋转
drawRectColor.drawSelf(r, g, b, a);// 绘制上面
MatrixState.popMatrix();// 恢复现场
MatrixState.pushMatrix();// 保护现场
MatrixState.translate(0, -Offset, 0);// 执行平移
MatrixState.rotate(90, 1, 0, 0);// 执行旋转
drawRectColor.drawSelf(r, g, b, a);// 绘制下面
MatrixState.popMatrix();// 恢复现场
MatrixState.pushMatrix();// 保护现场
MatrixState.translate(-Offset, 0, 0);// 执行平移
MatrixState.rotate(-90, 0, 1, 0);// 执行旋转
drawRectColor.drawSelf(r, g, b, a);// 绘制左面
MatrixState.popMatrix();// 恢复现场
MatrixState.pushMatrix();// 保护现场
MatrixState.translate(Offset, 0, 0);// 执行平移
MatrixState.rotate(90, 0, 1, 0);// 执行旋转
drawRectColor.drawSelf(r, g, b, a);// 绘制右面
MatrixState.popMatrix();// 恢复现场
MatrixState.pushMatrix();// 保护现场
MatrixState.translate(0, 0, Offset);// 执行平移
drawRectColor.drawSelf(r, g, b, a);// 绘制前面
MatrixState.popMatrix();// 恢复现场
MatrixState.pushMatrix();// 保护现场
MatrixState.translate(0, 0, -Offset);// 执行平移
MatrixState.rotate(180, 0, 1, 0);// 执行旋转
drawRectColor.drawSelf(r, g, b, a);// 绘制后面
MatrixState.popMatrix();// 恢复现场
GLES20.glDisable(GLES20.GL_CULL_FACE);
GLES20.glDisable(GLES20.GL_DEPTH_TEST);
}

}

package com.gzdxid.physicalworld;

import javax.vecmath.Vector3f;

import com.bulletphysics.collision.shapes.CollisionShape;
import com.bulletphysics.dynamics.DiscreteDynamicsWorld;
import com.bulletphysics.dynamics.RigidBody;
import com.bulletphysics.linearmath.DefaultMotionState;
import com.bulletphysics.linearmath.Transform;
import com.gzdxid.utils.DrawRectColor;

public class DW_RectColor {

private DrawRectColor drawRectColor;

public DW_RectColor(float size,float yOffset,CollisionShape collisionShape,
DiscreteDynamicsWorld dynamicsWorld,int mProgram) {
initRect(size,mProgram);
initPropertity( collisionShape, dynamicsWorld);
}

private void initRect(float size, int mProgram) {
// TODO Auto-generated method stub
drawRectColor=new DrawRectColor(size/2, size/2, mProgram);
}

private void initPropertity(CollisionShape collisionShape, DiscreteDynamicsWorld dynamicsWorld) {
// TODO Auto-generated method stub
Transform groundTransform=new Transform();
groundTransform.setIdentity();
groundTransform.origin.set(new Vector3f(0, -0.8f, 0));
Vector3f localInertia=new Vector3f(0, 0, 0);
DefaultMotionState motionState=new DefaultMotionState(groundTransform);
RigidBody groundBody=new RigidBody(0, motionState, collisionShape, localInertia);
groundBody.setRestitution(0.4f);
groundBody.setFriction(0.8f);
dynamicsWorld.addRigidBody(groundBody);
}

public void drawSelf(float r,float g,float b,float a){
drawRectColor.drawSelf(r, g, b, a);
}
}

package com.gzdxid.utils;

import javax.vecmath.Quat4f;

public class QuatUtil {

//将四元数转换为角度及转轴向量
public static float[] fromQUATtoAXYZ(Quat4f q4)
{
double sitaHalf=Math.acos(q4.w);
float nx=(float) (q4.x/Math.sin(sitaHalf));
float ny=(float) (q4.y/Math.sin(sitaHalf));
float nz=(float) (q4.z/Math.sin(sitaHalf));

return new float[]{(float) Math.toDegrees(sitaHalf*2),nx,ny,nz};
}
}