re-wrote rotation calculation

example/WiiMoteOpenGLDemo/Classes/EAGLView.h

@@ -39,13 +39,20 @@ Note that setting the view non-opaque will only work if the EAGL surface has an
 	
 	GLuint textures[1];
 	GLfloat rota;
+
+	BOOL useRotationMatrix;
 	
+	// use rotation matrix
+	GLfloat rotationMatrix[16];
+	
+	// use euler angles
 	int rotateX;
 	int rotateY;
 	int rotateZ;
 }
 
 @property NSTimeInterval animationInterval;
+@property BOOL useRotationMatrix;
 
 - (void)startAnimation;
 - (void)stopAnimation;
@@ -54,6 +61,7 @@ Note that setting the view non-opaque will only work if the EAGL surface has an
 - (void)setupView;
 - (void)checkGLError:(BOOL)visibleCheck;
 
+- (void)setRotationMatrix:(float[3][3]) matrix;
 - (void)setRotationX:(int)X Y:(int)Y Z:(int)Z;
 - (void)loadTexture;
 

example/WiiMoteOpenGLDemo/Classes/EAGLView.m

@@ -34,7 +34,7 @@
 @synthesize context;
 @synthesize animationTimer;
 @synthesize animationInterval;
-
+@synthesize useRotationMatrix;
 
 // You must implement this method
 + (Class)layerClass {
@@ -160,15 +160,20 @@
     glTranslatef(0.0, 0.0, -2.0);
 	
 #ifdef USE_BLUETOOTH
-	glRotatef(rotateX, 1.0f, 0.0f, 0.0f);
+	if (self.useRotationMatrix) {
-	glRotatef(rotateY, 0.0f, 1.0f, 0.0f);
+		glMultMatrixf(rotationMatrix);
-	glRotatef(rotateZ, 0.0f, 0.0f, 1.0f);
+	} else {
-	// 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);
+		// glRotatef(1.0f, rotateX, rotateY, rotateZ);
+	}
 #else
 	rota += 1;
 	glRotatef(rota, 0.0, 0.5, 0.0);
 	glRotatef(rota, 0.0, 0.0, 1.0);
 #endif
+	
 	glVertexPointer(3, GL_FLOAT, 0, cubeVertices);
     glEnableClientState(GL_VERTEX_ARRAY);
 
@@ -357,8 +362,35 @@
     }
 }
 
+- (void)setRotationMatrix:(float[3][3]) matrix{
+	useRotationMatrix = YES;
+	
+	// extend 3 by 3 matrix to 4 by 4
+	int pos = 0;
+	rotationMatrix[pos++] = matrix[0][0];
+	rotationMatrix[pos++] = matrix[0][1];
+	rotationMatrix[pos++] = matrix[0][2];
+	rotationMatrix[pos++] = 0;
+
+	rotationMatrix[pos++] = matrix[1][0];
+	rotationMatrix[pos++] = matrix[1][1];
+	rotationMatrix[pos++] = matrix[1][2];
+	rotationMatrix[pos++] = 0;
+
+	rotationMatrix[pos++] = matrix[2][0];
+	rotationMatrix[pos++] = matrix[2][1];
+	rotationMatrix[pos++] = matrix[2][2];
+	rotationMatrix[pos++] = 0;
+
+	rotationMatrix[pos++] = 0;
+	rotationMatrix[pos++] = 0;
+	rotationMatrix[pos++] = 0;
+	rotationMatrix[pos++] = 1;
+}
 
 - (void)setRotationX:(int)x Y:(int)y Z:(int)z{
+	useRotationMatrix = NO;
+
 	// NSLog(@"BT data: %u %u %u", x , y ,z);
 	rotateX = x;
 	rotateY = y;

example/WiiMoteOpenGLDemo/Classes/WiiMoteOpenGLDemoAppDelegate.m

@@ -45,6 +45,11 @@
 #import <btstack/run_loop.h>
 #import <btstack/hci_cmds.h>
 
+// quaternion rotation library
+float norm(float *vector, int dim);
+void normalizeVector(float *vector, int dim);
+void getRotationMatrixFromVectors(float vin[3], float vout[3], float matrix[3][3]);
+
 BTDevice *device;
 uint16_t wiiMoteConHandle = 0;
 WiiMoteOpenGLDemoAppDelegate * theMainApp;
@@ -59,120 +64,38 @@ WiiMoteOpenGLDemoAppDelegate * theMainApp;
 
 #define SIZE  5
 int counter;
+
+// define the rest position 
+static float restPosition[3] = {0,0,1};
+#define histSize 5
+int histX[histSize];
+int histY[histSize];
+int histZ[histSize];
+
+static float addToHistory(int history[histSize], int value){
+	int i;
+	float sum = 0;
+	for (i=0; i<histSize-1;i++){
+		history[i] = history[i+1];
+		sum += history[i];
+	}
+	history[histSize-1] = value;
+	return sum/histSize;
+}
+
 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 ax = x - 128;
+	float accData[3];
-	float ay = y - 128;
+	accData[0] = addToHistory( histX, 1 * (x - 128));
-	float az = z - 128;
+	accData[1] = addToHistory( histY, 1 * (y - 128));
-	
+	accData[2] = addToHistory( histZ, 1 * (z - 128));
-	// mini calib
-	// az /= 2;
-	
-#if 0
-	// normalize vector
-	float length = sqrt( ax*ax + ay*ay + az*az);
-	ax /= length;
-	ay /= length;
-	az /= length;
-#endif
 
-#if 0
+	float rotationMatrix[3][3];
-	// cross product between A and (0,0,1) 
+	getRotationMatrixFromVectors(restPosition, accData, rotationMatrix);
-	float crossX = ay;
-	float crossY = -ax;
-	float crossZ = 0;
-	float omega = acos( az );
-
-	// normalize quat
-	float quatSum = crossX * crossX + crossY * crossY + omega * omega;
-	crossX /= quatSum;
-	crossY /= quatSum;
-	crossZ /= quatSum;
-	omega  /= quatSum;
 	
-	//
+	[[theMainApp glView] setRotationMatrix:rotationMatrix];
-	int pitch = atan2( 2*(omega*crossX), omega*omega - crossX*crossX - crossY*crossY)* 180 / M_PI;
-	int roll  = atan2( 2*(crossX * crossY), omega*omega + crossX*crossX - crossY*crossY) * 180 / M_PI;
-	int theta = 0;
-#endif	
-#if 0	
-	int roll  = atan2(ax, sqrt(ay*ay+az*az)) * 180 / M_PI; 
-	int pitch = atan2(ay, sqrt(ax*ax+az*az)) * 180 / M_PI;
-	int theta = atan2(sqrt(ax*ax+ay*ay), az) * 180 / M_PI;
-	if (az < 0) {
-	 	pitch  = 180 - pitch;
-	}
-#endif
-	
-	// sort axes
-	float h = az;
-	az = -ay;
-	ay = h;
-
-	// calc
-	int roll =  atan2(ax, ay) * 180 / M_PI;
-	int pitch = atan2(ay, az) * 180 / M_PI;
-	int theta = 0;
-	
-#if 0
-	if (roll >= 90 || roll <= -90) {
-		pitch = 360 - pitch;
-	}
-	// if ( (++counter & 15) == 0)
-	// NSLog(@"BT data: %f %f %f: pitch %i, roll %i, yaw %i", ax , ay ,az, pitch, roll, theta);
-#endif
-	pitch = 0;
-	
-#if 1
-	static int lastPitch;
-	static int lastRoll;
-
-	if (abs(lastPitch - pitch) > 180) {
-		if (pitch > lastPitch) {
-			pitch -= 360;
-		} else {
-			pitch += 360;
-		}
-	}
-
-	if (abs(lastRoll - roll) > 180) {
-		if (roll > lastRoll) {
-			roll -= 360;
-		} else {
-			roll += 360;
-		}
-	}
-	
-	// moving average of size SIZE
-	static int pos = 0;
-	static int historyRoll[SIZE];
-	static int historyPitch[SIZE];
-	static int historyTheta[SIZE];
-	
-	
-	historyRoll[pos] = roll;
-	historyPitch[pos] = pitch;
-	historyTheta[pos] = theta;
-	pos++;
-	if (pos==SIZE) pos = 0;
-	
-	pitch = roll = 0;
-	int i;
-	for (i=0;i<SIZE;i++){
-		roll  += historyRoll[i];
-		pitch += historyPitch[i];
-		theta += historyTheta[i];
-	}
-	roll = roll / SIZE;
-	pitch = pitch / SIZE;
-	theta = theta / SIZE;
-	
-	lastPitch = pitch;
-	lastRoll  = roll;
-#endif	
-	// hack 
-	[[theMainApp glView] setRotationX:(-pitch) Y:roll Z:0];
 }
 
 void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){
@@ -203,8 +126,7 @@ void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint
 							// interupt channel openedn succesfully, now open control channel, too.
 							bt_send_cmd(&l2cap_create_channel, event_addr, 0x11);
 						} else {
-
+							// request acceleration data.. 
-							// request acceleration data.. probably has to be sent to control channel 0x11 instead of 0x13
 							uint8_t setMode31[] = { 0x52, 0x12, 0x00, 0x31 };
 							bt_send_l2cap( source_cid, setMode31, sizeof(setMode31));
 							uint8_t setLEDs[] = { 0x52, 0x11, 0x10 };

example/WiiMoteOpenGLDemo/WiiMoteOpenGLDemo.xcodeproj/project.pbxproj

@@ -21,6 +21,7 @@
 		9CB96EEF10278D8D002663D0 /* EAGLView.m in Sources */ = {isa = PBXBuildFile; fileRef = 28FD14FD0DC6FC130079059D /* EAGLView.m */; };
 		9CC8B5E51093727700BCBA1F /* wiimote_logo_55px.png in Resources */ = {isa = PBXBuildFile; fileRef = 9CC8B5E41093727700BCBA1F /* wiimote_logo_55px.png */; };
 		9CD1C867117E2F6900C7A4F4 /* libBTstack.dylib in Frameworks */ = {isa = PBXBuildFile; fileRef = 9CD1C866117E2F6900C7A4F4 /* libBTstack.dylib */; };
+		9CFFD60E11C5484300A37038 /* rotation.c in Sources */ = {isa = PBXBuildFile; fileRef = 9CFFD60D11C5484300A37038 /* rotation.c */; };
 /* End PBXBuildFile section */
 
 /* Begin PBXFileReference section */
@@ -51,6 +52,7 @@
 		9CB96E9710278945002663D0 /* CoreGraphics.framework */ = {isa = PBXFileReference; lastKnownFileType = wrapper.framework; name = CoreGraphics.framework; path = System/Library/Frameworks/CoreGraphics.framework; sourceTree = SDKROOT; };
 		9CC8B5E41093727700BCBA1F /* wiimote_logo_55px.png */ = {isa = PBXFileReference; lastKnownFileType = image.png; path = wiimote_logo_55px.png; sourceTree = "<group>"; };
 		9CD1C866117E2F6900C7A4F4 /* libBTstack.dylib */ = {isa = PBXFileReference; lastKnownFileType = "compiled.mach-o.dylib"; name = libBTstack.dylib; path = ../../src/libBTstack.dylib; sourceTree = SOURCE_ROOT; };
+		9CFFD60D11C5484300A37038 /* rotation.c */ = {isa = PBXFileReference; fileEncoding = 5; lastKnownFileType = sourcecode.c.c; path = rotation.c; sourceTree = "<group>"; };
 /* End PBXFileReference section */
 
 /* Begin PBXFrameworksBuildPhase section */
@@ -109,6 +111,7 @@
 		29B97315FDCFA39411CA2CEA /* Other Sources */ = {
 			isa = PBXGroup;
 			children = (
+				9CFFD60D11C5484300A37038 /* rotation.c */,
 				32CA4F630368D1EE00C91783 /* WiiMoteOpenGLDemo_Prefix.pch */,
 				29B97316FDCFA39411CA2CEA /* main.m */,
 			);
@@ -221,6 +224,7 @@
 				9C0D06391091035200FC3BBA /* BTDevice.m in Sources */,
 				9C0D063A1091035200FC3BBA /* BTInquiryViewController.m in Sources */,
 				9C0D070D1092316D00FC3BBA /* EAGLViewController.m in Sources */,
+				9CFFD60E11C5484300A37038 /* rotation.c in Sources */,
 			);
 			runOnlyForDeploymentPostprocessing = 0;
 		};
@@ -274,7 +278,7 @@
 			isa = XCBuildConfiguration;
 			buildSettings = {
 				ARCHS = "$(ARCHS_STANDARD_32_BIT)";
-				"CODE_SIGN_IDENTITY[sdk=iphoneos*]" = "Don't Code Sign";
+				"CODE_SIGN_IDENTITY[sdk=iphoneos*]" = "iPhone Developer: Milanka Ringwald (Y4MWH89GZ9)";
 				GCC_C_LANGUAGE_STANDARD = c99;
 				GCC_WARN_ABOUT_RETURN_TYPE = YES;
 				GCC_WARN_UNUSED_VARIABLE = YES;
@@ -286,6 +290,7 @@
 				OTHER_CPLUSPLUSFLAGS = "";
 				OTHER_LDFLAGS = "";
 				PREBINDING = NO;
+				"PROVISIONING_PROFILE[sdk=iphoneos*]" = "86D3FD5B-5B72-4D87-88B3-0737B204F98E";
 				SDKROOT = iphoneos2.0;
 				USER_HEADER_SEARCH_PATHS = "";
 			};
@@ -295,7 +300,7 @@
 			isa = XCBuildConfiguration;
 			buildSettings = {
 				ARCHS = "$(ARCHS_STANDARD_32_BIT)";
-				"CODE_SIGN_IDENTITY[sdk=iphoneos*]" = "Don't Code Sign";
+				"CODE_SIGN_IDENTITY[sdk=iphoneos*]" = "iPhone Developer: Milanka Ringwald (Y4MWH89GZ9)";
 				GCC_C_LANGUAGE_STANDARD = c99;
 				GCC_WARN_ABOUT_RETURN_TYPE = YES;
 				GCC_WARN_UNUSED_VARIABLE = YES;
@@ -306,6 +311,7 @@
 				OTHER_CPLUSPLUSFLAGS = "";
 				OTHER_LDFLAGS = "";
 				PREBINDING = NO;
+				"PROVISIONING_PROFILE[sdk=iphoneos*]" = "86D3FD5B-5B72-4D87-88B3-0737B204F98E";
 				SDKROOT = iphoneos2.0;
 				USER_HEADER_SEARCH_PATHS = "";
 			};

example/WiiMoteOpenGLDemo/rotation.c

@@ -0,0 +1,169 @@
+#include <stdio.h>
+#include <string.h>
+#include <math.h>
+
+#define rad M_PI/180.0
+#define deg 180.0/M_PI
+
+// define the rest position 
+static float restPosition[3] = {1,0,0};
+
+float norm(float *vector, int dim){
+    float result = 0;
+    int i;
+    
+    for (i=0; i<dim; i++){
+        result+= vector[i]*vector[i];
+    }
+    
+    if (result == 0) result = 1;
+    return sqrt(result);
+}
+
+void normalizeVector(float *vector, int dim){
+    int i;
+    float vnorm = norm(vector,dim);
+    for (i=0; i<dim; i++){
+        vector[i]/=vnorm;
+    }
+}
+
+/****** START OF TEST FUNCTIONS THAT CREATE ACC_DATA ******/
+
+// define X and Y axes 
+static float rotationAxisX[3] = {0,0,-1};
+static float rotationAxisY[3] = {1,0,0};
+
+static void quaternionFromAxis(float angle, float axis[3], float quaternion[4]){
+    normalizeVector(axis,3);
+	
+	float qangle = angle * 0.5;
+	float sinQAngle = sin(qangle);
+ 
+    quaternion[0] = cos(qangle);
+    quaternion[1] = axis[0]*sinQAngle;
+    quaternion[2] = axis[1]*sinQAngle;
+    quaternion[3] = axis[2]*sinQAngle;
+}
+
+
+static void multiplyQuaternions(float qy[4], float qx[4], float result[4]){
+    // p0*q2 - p*q + p0*q + q0*p + pxq
+    
+    float t0 = (qy[3]-qy[2])*(qx[2]-qx[3]);
+    float t1 = (qy[0]+qy[1])*(qx[0]+qx[1]);
+    float t2 = (qy[0]-qy[1])*(qx[2]+qx[3]);
+    float t3 = (qy[3]+qy[2])*(qx[0]-qx[1]);
+    float t4 = (qy[3]-qy[1])*(qx[1]-qx[2]);
+    float t5 = (qy[3]+qy[1])*(qx[1]+qx[2]);
+    float t6 = (qy[0]+qy[2])*(qx[0]-qx[3]);
+    float t7 = (qy[0]-qy[2])*(qx[0]+qx[3]);
+    float t8 = t5+t6+t7;
+    float t9 = (t4+t8)/2;
+     
+    result[0] = t0+t9-t5;
+    result[1] = t1+t9-t8;
+    result[2] = t2+t9-t7;
+    result[3] = t3+t9-t6;
+}
+
+static void rotateVectorWithQuaternion(float restPosition[3], float qrot[4], float qaxis[3]){
+    float result[4] = {0,0,0,0};
+    
+    float qrot_conj[4] = {qrot[0], -qrot[1], -qrot[2], -qrot[3]};
+    float qrestPosition[4] = {0, restPosition[0], restPosition[1], restPosition[2]};
+    
+    multiplyQuaternions(qrestPosition,qrot_conj,result);
+    multiplyQuaternions(qrot,result,result);
+    
+    qaxis[0] = result[1];
+    qaxis[1] = result[2];
+    qaxis[2] = result[3];
+}
+    
+static void getAccellerometerData(float radrotationAngleX, float radrotationAngleY, float qaxis[3]){
+    float qx[4] = {0,0,0,0};
+    float qy[4] = {0,0,0,0};
+    float qrot[4] = {0,0,0,0};
+    
+    quaternionFromAxis(radrotationAngleX,rotationAxisX,qx);
+    quaternionFromAxis(radrotationAngleY,rotationAxisY,qy);
+    
+    multiplyQuaternions(qy,qx,qrot);
+    rotateVectorWithQuaternion(restPosition,qrot,qaxis);
+} 
+
+/****** END OF TEST FUNCTIONS THAT CREATE ACC_DATA ******/
+
+
+void getRotationMatrixFromQuartenion(float q[4], float m[3][3]){
+    float w = q[0];
+    float x = q[1];
+    float y = q[2];
+    float z = q[3];
+    
+    float y2 = y*y; 
+    float x2 = x*x; 
+    float z2 = z*z;
+    
+    m[0][0] = 1-2*y2-2*z2;
+    m[0][1] = 2*x*y-2*w*z;
+    m[0][2] = 2*x*z+2*w*y;
+    
+    m[1][0] = 2*x*y+2*w*z;
+    m[1][1] = 1-2*x2-2*z2;
+    m[1][2] = 2*y*z-2*w*x;
+    
+    m[2][0] = 2*x*z-2*w*y;
+    m[2][1] = 2*y*z+2*w*x;
+    m[2][2] = 1-2*x2-2*y2;
+}
+
+
+void getRotationMatrixFromVectors(float vin[3], float vout[3], float matrix[3][3]){
+    normalizeVector(vout,3);
+    
+    float q[4] = {0,0,0,0};
+    
+    float vin_length = 0;
+    float vout_length = 0;
+    float dotprod = 0;
+    
+    int i;
+    for (i=0; i<3; i++){
+        vin_length += vin[i]*vin[i];
+        vout_length+= vout[i]*vout[i];
+        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] = -1*(vin[0]*vout[1] - vin[1]*vout[0]);
+    
+    normalizeVector(q,4);
+    
+    getRotationMatrixFromQuartenion(q,matrix);
+}
+
+#if 0
+int main(void)
+{
+    float accData[3] = {1,0,0};
+    float rotationMatrix[3][3];
+    
+    normalizeVector(restPosition,3);
+    
+    int angle;
+    for (angle = 0; angle <90; angle+=30){
+        getAccellerometerData(angle*rad*1.5,angle*rad,accData);
+        
+        getRotationMatrixFromVectors(restPosition, accData, rotationMatrix);
+    }
+    
+	return 1;
+}
+#endif
+
+
+