From a0232ae94b766101ec59ee569085a074c93e84d4 Mon Sep 17 00:00:00 2001 From: "matthias.ringwald" Date: Sun, 13 Jun 2010 20:55:38 +0000 Subject: [PATCH] more tries to stabilize object when upside down --- example/WiiMoteOpenGLDemo/Classes/EAGLView.m | 9 ++---- .../Classes/WiiMoteOpenGLDemoAppDelegate.m | 32 ++++++++++++++++--- example/WiiMoteOpenGLDemo/rotation.c | 24 ++++++++------ 3 files changed, 44 insertions(+), 21 deletions(-) diff --git a/example/WiiMoteOpenGLDemo/Classes/EAGLView.m b/example/WiiMoteOpenGLDemo/Classes/EAGLView.m index 9e2ccb081..508ce8e8a 100644 --- a/example/WiiMoteOpenGLDemo/Classes/EAGLView.m +++ b/example/WiiMoteOpenGLDemo/Classes/EAGLView.m @@ -162,12 +162,10 @@ #ifdef USE_BLUETOOTH if (self.useRotationMatrix) { glMultMatrixf(rotationMatrix); - } else { - glRotatef(rotateX, 1.0f, 0.0f, 0.0f); - glRotatef(rotateY, 0.0f, 1.0f, 0.0f); - glRotatef(rotateZ, 0.0f, 0.0f, 1.0f); - // glRotatef(1.0f, rotateX, rotateY, rotateZ); } + glRotatef(rotateX, 1.0f, 0.0f, 0.0f); + glRotatef(rotateY, 0.0f, 1.0f, 0.0f); + glRotatef(rotateZ, 0.0f, 0.0f, 1.0f); #else rota += 1; glRotatef(rota, 0.0, 0.5, 0.0); @@ -373,7 +371,6 @@ } - (void)setRotationX:(int)x Y:(int)y Z:(int)z{ - useRotationMatrix = NO; // NSLog(@"BT data: %u %u %u", x , y ,z); rotateX = x; diff --git a/example/WiiMoteOpenGLDemo/Classes/WiiMoteOpenGLDemoAppDelegate.m b/example/WiiMoteOpenGLDemo/Classes/WiiMoteOpenGLDemoAppDelegate.m index 27930e400..e9d4bede5 100644 --- a/example/WiiMoteOpenGLDemo/Classes/WiiMoteOpenGLDemoAppDelegate.m +++ b/example/WiiMoteOpenGLDemo/Classes/WiiMoteOpenGLDemoAppDelegate.m @@ -49,7 +49,8 @@ float norm(float *vector, int dim); void normalizeVector(float *vector, int dim); void getRotationMatrixFromVectors(float vin[3], float vout[3], float matrix[4][4]); - +float getRotationAngle(float matrix[4][4]); + BTDevice *device; uint16_t wiiMoteConHandle = 0; WiiMoteOpenGLDemoAppDelegate * theMainApp; @@ -67,6 +68,7 @@ int counter; // define the rest position static float restPosition[3] = {0,0,1}; +static float restPosition2[3] = {0,0,-1}; #define histSize 5 int histX[histSize]; int histY[histSize]; @@ -85,18 +87,38 @@ static float addToHistory(int history[histSize], int value){ static void bt_data_cb(uint8_t x, uint8_t y, uint8_t z){ - // NSLog(@"BT data: %u %u %u", x , y ,z); - // [[theMainApp status] setText:[NSString stringWithFormat:@"X:%03u Y:%03u Z:%03u", x, y, z]]; float accData[3]; accData[0] = addToHistory( histX, 1 * (x - 128)); accData[1] = addToHistory( histY, 1 * (y - 128)); accData[2] = addToHistory( histZ, 1 * (z - 128)); - - printf("%f, %f, %f\n", accData[0], accData[1], accData[2]); float rotationMatrix[4][4]; getRotationMatrixFromVectors(restPosition, accData, rotationMatrix); + + float rotationAngle = getRotationAngle(rotationMatrix) * 180/M_PI; +#if 0 + if (rotationAngle >= 90){ + getRotationMatrixFromVectors(restPosition2, accData, rotationMatrix); + [[theMainApp glView] setRotationX:0 Y:180 Z:0]; + } else { + [[theMainApp glView] setRotationX:0 Y:0 Z:0]; + } +#endif +#if 1 + // float frontV[3] ={ 1, 0, 0}; + float projectectFrontV[3]; + projectectFrontV[0] = rotationMatrix[0][0]; + projectectFrontV[1] = rotationMatrix[1][0]; + projectectFrontV[2] = rotationMatrix[2][0]; + float correctionZ = atan2(projectectFrontV[1], projectectFrontV[0]) * 180/M_PI; + printf("%f, %f, %f - angle %f - dir %f, %f=> %f\n", accData[0], accData[1], accData[2], rotationAngle, + projectectFrontV[0], projectectFrontV[1], correctionZ); + // if (rotationAngle >= 90){ + // [[theMainApp glView] setRotationX:0 Y:0 Z:-correctionZ]; + // } + +#endif [[theMainApp glView] setRotationMatrix:rotationMatrix]; } diff --git a/example/WiiMoteOpenGLDemo/rotation.c b/example/WiiMoteOpenGLDemo/rotation.c index 4186f4d4e..4a0ffe1ea 100644 --- a/example/WiiMoteOpenGLDemo/rotation.c +++ b/example/WiiMoteOpenGLDemo/rotation.c @@ -143,20 +143,24 @@ void getRotationMatrixFromVectors(float vin[3], float vout[3], float matrix[4][4 vout_length+= vout[i]*vout[i]; dotprod += vin[i]*vout[i]; } - - //q[0] = sqrt(vin_length * vout_length) + dotprod; - + +#if 1 //mila + 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] = -1*(vin[0]*vout[1] - vin[1]*vout[0]); +#else 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]); + axis[0] = -1*(vin[1]*vout[2] - vin[2]*vout[1]); + axis[1] = -1*(vin[2]*vout[0] - vin[0]*vout[2]); + axis[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); - + quaternionFromAxis(angle, axis, q); +#endif + + normalizeVector(q,4); getRotationMatrixFromQuartenion(q,matrix); }