int testRotation(double * quaternions , double * euler , double rX,double rY,double rZ)
{
unsigned int i=0;
signed int inc=10;
for (i=0; i<5; i++)
{
quaternionRotate(quaternions,rX,rY,rZ, inc,qXqYqZqW);
quaternions2Euler(euler,quaternions,qXqYqZqW);
printf("Step %u %d | %f %f %f \n",i,inc,euler[0],euler[1],euler[2]);
}
inc=-10;
for (i=0; i<5; i++)
{
quaternionRotate(quaternions,rX,rY,rZ, inc,qXqYqZqW);
quaternions2Euler(euler,quaternions,qXqYqZqW);
printf("Step %u %d | %f %f %f \n",i,inc,euler[0],euler[1],euler[2]);
}
return 1;
}
int convertQuaternionsToEulerAngles(struct VirtualStream * stream,double * euler,double *quaternion)
{
normalizeQuaternions(&quaternion[0],&quaternion[1],&quaternion[2],&quaternion[3]);
double eulerTMP[3];
quaternions2Euler(eulerTMP,quaternion,1); //1
euler[0] = stream->rotationsOffset[0] + (stream->scaleWorld[3] * eulerTMP[0]);
euler[1] = stream->rotationsOffset[1] + (stream->scaleWorld[4] * eulerTMP[1]);
euler[2] = stream->rotationsOffset[2] + (stream->scaleWorld[5] * eulerTMP[2]);
#if PRINT_LOAD_INFO
fprintf(stderr,"Tracker OBJ%u( %f %f %f , %f %f %f )\n",item,pos[0],pos[1],pos[2],pos[3],pos[4],pos[5]);
fprintf(stderr,"Angle Offset %f %f %f \n",newstream->rotationsOffset[0],newstream->rotationsOffset[1],newstream->rotationsOffset[2]);
#endif
if (stream->rotationsOverride)
{ flipRotationAxisD(&euler[0],&euler[1],&euler[2], stream->rotationsXYZ[0] , stream->rotationsXYZ[1] , stream->rotationsXYZ[2]); }
return 1;
}
