根据Android 传感器控制3D模型方向

目标

根据传感器信息, 平滑的控制3D模型方向.

实现

1. 注册传感器

@Override
	protected void onResume() {
		super.onResume();
		
		mSensorManager = (SensorManager) getSystemService(Context.SENSOR_SERVICE);
		// 初始化加速度传感器
		mAccelerometer = mSensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER);
		// 初始化地磁场传感器
		mMagnetic = mSensorManager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD);

		
		if (mSensorManager != null) {
			mSensorManager.registerListener(this, mAccelerometer, SensorManager.SENSOR_DELAY_GAME);
			mSensorManager.registerListener(this, mMagnetic, SensorManager.SENSOR_DELAY_GAME);
		}
		
		
	}

2. 反注册

protected void onPause() {
		super.onPause();
		mSensorManager.unregisterListener(this);
	} 

3.  采样，计算旋转矩阵，再转换为四元数。

<span style="white-space:pre">	</span>float[] accelerometerValues = new float[3];
	float[] magneticFieldValues = new float[3];
	
	
	
	float getMat3(float[] m, int x, int y){
		return m[x + y * 3];
	}
	void matToQuat(float[]mat, float[]q) 
	{
		float		trace;
		float		s;
		float		t;
		int     	i;
		int			j;
		int			k;

		int 	next[] = { 1, 2, 0 };

		trace = mat[0] + mat[4] + mat[8];

		if ( trace > 0.0f ) 
		{
			t = trace + 1.0f;
			s = 0.5f / (float)Math.sqrt(t);
			q[3] = s * t;
			q[0] = ( getMat3(mat,  2 , 1 ) - getMat3(mat,  1 , 2 ) ) * s;
			q[1] = ( getMat3(mat,  0 , 2 ) - getMat3(mat,  2 , 0 ) ) * s;
			q[2] = ( getMat3(mat,  1 , 0 ) - getMat3(mat,  0 , 1 ) ) * s;

		} else {
			i = 0;
			if ( getMat3(mat, 1 , 1 ) > getMat3(mat, 0 , 0 ) ) {
				i = 1;
			}
			if ( getMat3(mat, 2 , 2 )> getMat3(mat, i , i ) ) {
				i = 2;
			}
			j = next[ i ];
			k = next[ j ];
			t = ( getMat3(mat, i, i ) - ( getMat3(mat, j , j ) + getMat3(mat, k , k ) ) ) + 1.0f;
			s = 0.5f / (float)Math.sqrt(t);
			q[i] = s * t;
			q[3] = ( getMat3(mat, k , j ) - getMat3(mat, j ,k ) ) * s;
			q[j] = ( getMat3(mat, j , i ) + getMat3(mat, i , j ) ) * s;
			q[k] = ( getMat3(mat, k , i ) + getMat3(mat, i , k ) ) * s;
		}
	}
	
	float rotMat[] = new float[9];
	float quat[] = new float[4];
	@Override
	public void onSensorChanged(SensorEvent event) {
		 
		// TODO Auto-generated method stub
		boolean needComputer = false;
		if (event.sensor.getType() == Sensor.TYPE_ACCELEROMETER) {
			//Log.e("ACCELEROMETER","onSensorChanged:TYPE_ACCELEROMETER " + String.format("%f %f %f", event.values[0],event.values[1],event.values[2]));
			

			
			if(true)
			{	
				accelerometerValues[0] = -event.values[1];
				accelerometerValues[1] = event.values[0];
				accelerometerValues[2] = event.values[2];
				needComputer = true;
			}			
		}
		if (event.sensor.getType() == Sensor.TYPE_MAGNETIC_FIELD) {

			
			if(true)
			{
				magneticFieldValues[0] = -event.values[1];
				magneticFieldValues[1] = event.values[0]; 
				magneticFieldValues[2] = event.values[2];
				needComputer = true;
			}
		}
		
		if(needComputer == true){
			SensorManager.getRotationMatrix(rotMat, null, accelerometerValues,
				magneticFieldValues);
			
			matToQuat(rotMat, mQuat);

		}

	}
	

4. 计算将几何模型坐标变换到手机世界坐标矩阵， 将上一部得到的四元数与当前四元数插值再转换为矩阵，计算视点变换矩阵

void normalizeQuat(float[] q){
		float temp=(float)Math.sqrt(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]);
		temp=(float)1.0/temp;
		q[0]*=temp;
		q[1]*=temp;
		q[2]*=temp;
		q[3]*=temp;
	}
	
	void quatSlerp(float[] q, float[]q0,float[] q1,float t) {
		float k0,k1,cosomega = q0[0] * q1[0] + q0[1] * q1[1] + q0[2] * q1[2] + q0[3] * q1[3];
		if(cosomega < 0.0) {
			cosomega = -cosomega;
			q[0] = -q1[0];
			q[1] = -q1[1];
			q[2] = -q1[2];
			q[3] = -q1[3];
		} else {
			q[0] = q1[0];
			q[1] = q1[1];
			q[2] = q1[2];
			q[3] = q1[3];
		}
		if(1.0 - cosomega > 1e-6) {
			float omega = (float)Math.acos(cosomega);
			float sinomega = (float)Math.sin(omega);
			k0 = (float)Math.sin((1.0f - t) * omega) / sinomega;
			k1 = (float)Math.sin(t * omega) / sinomega;
		} else {
			k0 = 1.0f - t;
			k1 = t;
		}
		q[0] = q0[0] * k0 + q[0] * k1;
		q[1] = q0[1] * k0 + q[1] * k1;
		q[2] = q0[2] * k0 + q[2] * k1;
		q[3] = q0[3] * k0 + q[3] * k1;
		
		normalizeQuat(q);
	}
	
	void quatToMat(float[] ret, float[]q) {
		float x = q[0];
		float y = q[1];
		float z = q[2];
		float w = q[3];
		
		float x2 = x + x;
		float y2 = y + y;
		float z2 = z + z;
		float xx = x * x2;
		float yy = y * y2;
		float zz = z * z2;
		float xy = x * y2;
		float yz = y * z2;
		float xz = z * x2;
		float wx = w * x2;
		float wy = w * y2;
		float wz = w * z2;
		ret[0] = 1.0f - (yy + zz); ret[4] = xy - wz;          ret[8] = xz + wy;           ret[12]=0.0f;
		ret[1] = xy + wz;          ret[5] = 1.0f - (xx + zz); ret[9] = yz - wx;           ret[13]=0.0f;
		ret[2] = xz - wy;          ret[6] = yz + wx;          ret[10]= 1.0f - (xx + yy);  ret[14]=0.0f;
		ret[3] = 0.0f;              ret[7]=  0.0f;              ret[11]= 0.0f;               ret[15]=1.0f;
	}

	
	float[] mCurQuat = new float[4];
	float[] mPreQuat = {0f, 0, 0, 1};
	float[] mRotMat = new float[16];
	
	public void draw() {
	...................................
		
	    temp[0] = 1.f;	temp[4] = 0.f;	temp[8] = 0.f;	temp[12] = 0.f;
	    temp[1] = 0.f;	temp[5] = 0.f;	temp[9] = -1.f;	temp[13] = 0.f;
	    temp[2] = 0.f;	temp[6] = 1.f;	temp[10] = 0.f;	temp[14] = 0.f;
	    temp[3] = 0.f;	temp[7] = 0.f;	temp[11] = 0.f;	temp[15] = 1.f;
	    
	    quatSlerp(mCurQuat, mPreQuat, rotMat, rotMat[4]);
	    
	    
	    mPreQuat[0] = mCurQuat[0];
	    mPreQuat[1] = mCurQuat[1];
	    mPreQuat[2] = mCurQuat[2];
	    mPreQuat[3] = mCurQuat[3];
	    quatToMat(mRotMat, mCurQuat);
	    
	    Matrix.multiplyMM(mWorld, 0, mRotMat, 0, temp, 0);
	    
		
	   ..............................................
	}