mirror/btstack
1
0
Fork 0
mirror of https://github.com/bluekitchen/btstack.git synced 2025-03-26 02:37:41 +00:00
Code Issues Packages Projects Releases Wiki Activity

reordered and fixed compile errors

Browse Source
This commit is contained in:
matthias.ringwald 2010-06-13 21:50:49 +00:00
parent 69025de84f
commit 7b5a7a7af9
1 changed files with 42 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

95
example/WiiMoteOpenGLDemo/rotation.c
View File

@ -128,6 +128,51 @@ void getRotationMatrixFromQuartenion(float q[4], float m[4][4]){
} }
float getRotationAngle(float matrix[4][4]){
return acos( (matrix[0][0]+matrix[1][1]+matrix[2][2]-1) * 0.5);
}
void slerp(float v0[4], float v1[4], double t, float result[4]) {
int i;
// http://number-none.com/product/Understanding%20Slerp,%20Then%20Not%20Using%20It/
// v0 and v1 should be unit length or else
// something broken will happen.
normalizeVector(v0,4);
normalizeVector(v1,4);
// Compute the cosine of the angle between the two vectors.
double dot = v0[0]*v1[0] + v0[1]*v1[1] + v0[2]*v1[2] + v0[3]*v1[3];
const double DOT_THRESHOLD = 0.9995;
if (dot > DOT_THRESHOLD) {
// If the inputs are too close for comfort, linearly interpolate
// and normalize the result.
for (i=0; i<4; i++){
result[i] = v0[i] + t*(v1[i] - v0[i]);
}
normalizeVector(result,4);
return;
}
if (dot<-1) dot = -1;
if (dot>1 ) dot = 1; // Robustness: Stay within domain of acos()
double theta_0 = acos(dot); // theta_0 = angle between input vectors
double theta = theta_0*t; // theta = angle between v0 and result
float v2[4] = {0,0,0,0};
for (i=0; i<4; i++){
v2[i] = v1[i] - v0[i]*dot;
}
normalizeVector(v2,4); // { v0, v2 } is now an orthonormal basis
for (i=0; i<4; i++){
result[i] = v0[i]*cos(theta) + v2[i]*sin(theta);
}
return;
}
void getRotationMatrixFromVectors(float vin[3], float vout[3], float matrix[4][4]){ void getRotationMatrixFromVectors(float vin[3], float vout[3], float matrix[4][4]){
normalizeVector(vout,3); normalizeVector(vout,3);
@ -160,58 +205,8 @@ void getRotationMatrixFromVectors(float vin[3], float vout[3], float matrix[4][4
quaternionFromAxis(angle, axis, q); quaternionFromAxis(angle, axis, q);
#endif #endif
float v0[4] = {0, vin[0], vin[1], vin[2]};
float v1[4] = {0, vout[0], vout[1], vout[2]};
slerp(v0, v1, 0.5, q);
normalizeVector(q,4); normalizeVector(q,4);
getRotationMatrixFromQuartenion(q,matrix); getRotationMatrixFromQuartenion(q,matrix);
}
float getRotationAngle(float matrix[4][4]){
return acos( (matrix[0][0]+matrix[1][1]+matrix[2][2]-1) * 0.5);
}
void slerp(float v0[4], float v1[4], double t, float result[4]) {
int i;
// http://number-none.com/product/Understanding%20Slerp,%20Then%20Not%20Using%20It/
// v0 and v1 should be unit length or else
// something broken will happen.
normalizeVector(v0,4);
normalizeVector(v1,4);
// Compute the cosine of the angle between the two vectors.
double dot = v0[0]*v1[0] + v0[1]*v1[1] + v0[2]*v1[2] + v0[3]*v1[3];
const double DOT_THRESHOLD = 0.9995;
if (dot > DOT_THRESHOLD) {
// If the inputs are too close for comfort, linearly interpolate
// and normalize the result.
for (i=0; i<4; i++){
result[i] = v0[i] + t*(v1[i] Ð v0[i]);
}
normalizeVector(result,4);
return result;
}
if (dot<-1) dot = -1;
if (dot>1 ) dot = 1; // Robustness: Stay within domain of acos()
double theta_0 = acos(dot); // theta_0 = angle between input vectors
double theta = theta_0*t; // theta = angle between v0 and result
float v2[4] = {0,0,0,0};
for (i=0; i<4; i++){
v2[i] = v1[i] Ð v0[i]*dot;
}
normalizeVector(v2,4); // { v0, v2 } is now an orthonormal basis
for (i=0; i<4; i++){
result[i] = v0[i]*cos(theta) + v2[i]*sin(theta);
}
return result;
} }
#if 0 #if 0