mirror/btstack
1
0
Fork 0
mirror of https://github.com/bluekitchen/btstack.git synced 2025-03-25 16:43:28 +00:00
Code Issues Packages Projects Releases Wiki Activity

more tries to stabilize object when upside down

Browse Source
This commit is contained in:
matthias.ringwald 2010-06-13 20:55:38 +00:00
parent fc7d768736
commit a0232ae94b
3 changed files with 44 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
example/WiiMoteOpenGLDemo/Classes/EAGLView.m
View File

@ -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;

32
example/WiiMoteOpenGLDemo/Classes/WiiMoteOpenGLDemoAppDelegate.m
View File

@ -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];
}

24
example/WiiMoteOpenGLDemo/rotation.c
View File

@ -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);
}