diff --git a/example/WiiMoteOpenGLDemo/rotation.c b/example/WiiMoteOpenGLDemo/rotation.c index decb2e280..4186f4d4e 100644 --- a/example/WiiMoteOpenGLDemo/rotation.c +++ b/example/WiiMoteOpenGLDemo/rotation.c @@ -144,14 +144,21 @@ void getRotationMatrixFromVectors(float vin[3], float vout[3], float matrix[4][4 dotprod += vin[i]*vout[i]; } - q[0] = sqrt(vin_length * vout_length) + dotprod; - q[1] = -1*(vin[1]*vout[2] - vin[2]*vout[1]); - q[2] = -1*(vin[2]*vout[0] - vin[0]*vout[2]); - q[3] = 0;//-1*(vin[0]*vout[1] - vin[1]*vout[0]); + //q[0] = sqrt(vin_length * vout_length) + dotprod; + float axis[3] = {0,0,0}; + a[0] = -1*(vin[1]*vout[2] - vin[2]*vout[1]); + a[1] = -1*(vin[2]*vout[0] - vin[0]*vout[2]); + a[2] = -1*(vin[0]*vout[1] - vin[1]*vout[0]); + normalizeVector(axis,3); + + float angle = acos(vin[0]*vout[0]+vin[1]*vout[1]+vin[2]*vout[2]); + + quaternionFromAxis(angle, axis[3], q); normalizeVector(q,4); getRotationMatrixFromQuartenion(q,matrix); + } float getRotationAngle(float matrix[4][4]){