mirror of
https://github.com/bluekitchen/btstack.git
synced 2025-04-16 08:42:28 +00:00
re-wrote rotation calculation
This commit is contained in:
matthias.ringwald
parent
5a2d670020
commit
4e49a8096b
@ -40,12 +40,19 @@ 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;
|
||||
|
||||
|
||||
@ -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
|
||||
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);
|
||||
}
|
||||
#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;
|
||||
|
||||
@ -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 ay = y - 128;
|
||||
float az = z - 128;
|
||||
float accData[3];
|
||||
accData[0] = addToHistory( histX, 1 * (x - 128));
|
||||
accData[1] = addToHistory( histY, 1 * (y - 128));
|
||||
accData[2] = addToHistory( histZ, 1 * (z - 128));
|
||||
|
||||
// mini calib
|
||||
// az /= 2;
|
||||
float rotationMatrix[3][3];
|
||||
getRotationMatrixFromVectors(restPosition, accData, rotationMatrix);
|
||||
|
||||
#if 0
|
||||
// normalize vector
|
||||
float length = sqrt( ax*ax + ay*ay + az*az);
|
||||
ax /= length;
|
||||
ay /= length;
|
||||
az /= length;
|
||||
#endif
|
||||
|
||||
#if 0
|
||||
// cross product between A and (0,0,1)
|
||||
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;
|
||||
|
||||
//
|
||||
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];
|
||||
[[theMainApp glView] setRotationMatrix:rotationMatrix];
|
||||
}
|
||||
|
||||
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.. probably has to be sent to control channel 0x11 instead of 0x13
|
||||
// request acceleration data..
|
||||
uint8_t setMode31[] = { 0x52, 0x12, 0x00, 0x31 };
|
||||
bt_send_l2cap( source_cid, setMode31, sizeof(setMode31));
|
||||
uint8_t setLEDs[] = { 0x52, 0x11, 0x10 };
|
||||
|
||||
@ -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 = "";
|
||||
};
|
||||
|
||||
169
example/WiiMoteOpenGLDemo/rotation.c
Normal file
169
example/WiiMoteOpenGLDemo/rotation.c
Normal file
@ -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
|
||||
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user