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);
}
/* Update transformation matrix R using quaternion multiplication */
GLvoid updateR (GLubyte c, GLdouble d, GLdouble *a, GLdouble *u)
{
/* set quaternion R */
GLdouble R[4] = {0.0};
GLdouble radA = (*a)/180.0*PI;
R[0] = cos(radA/2);
R[1] = sin(radA/2)*u[0];
R[2] = sin(radA/2)*u[1];
R[3] = sin(radA/2)*u[2];
/* set quaternion Q */
GLdouble Q[4] = {0.0};
GLdouble radD = d/180.0*PI;
Q[0] = cos(radD/2);
switch(c)
{
case 'x':
case 'X':
case GLUT_KEY_UP:
case GLUT_KEY_DOWN:
Q[1] = sin(radD/2)*1.0;
break;
case 'y':
case 'Y':
case GLUT_KEY_LEFT:
case GLUT_KEY_RIGHT:
Q[2] = sin(radD/2)*1.0;
break;
case 'z':
case 'Z':
Q[3] = sin(radD/2)*1.0;
break;
default:
break;
}
/* quaternion multiplication */
multiplyQuaternions(Q, R);
/* set return value. Due to machine precision, sometimes R[0] is a little out of
range [-1,1], but the good news is sin(radA_2) cannot be absolutely zero! */
GLdouble radA_2 = acos(R[0]>1.0?1.0:R[0]);
if (fabs(sin(radA_2))>1e-8)
{
u[0] = R[1]/sin(radA_2);
u[1] = R[2]/sin(radA_2);
u[2] = R[3]/sin(radA_2);
}
(*a) = 2*radA_2/PI*180;
}
Quaternion& Quaternion::multiply( const Quaternion& q ) {
return multiplyQuaternions( *this, q );
}
