Code: Select all
btVector3 getEuler(btQuaternion q)
{
//get those euler angles(x,y,z) and put them in a vector
return vector;
}
Code: Select all
btVector3 getEuler(btQuaternion q)
{
//get those euler angles(x,y,z) and put them in a vector
return vector;
}
Code: Select all
btVector3 quatToEuler(btQuaternion quat)
{
float heading,attitude,bank;
Quaternion q1;
q1.x =quat.getX();
q1.y = quat.getY();
q1.z = quat.getZ();
q1.w = quat.getW();
double test = q1.x*q1.y + q1.z*q1.w;
if (test > 0.499) { // singularity at north pole
heading = 2 * atan2(q1.x,q1.w);
attitude = PI/2;
bank = 0;
return btVector3(0,0,0);
}
if (test < -0.499) { // singularity at south pole
heading = -2 * atan2(q1.x,q1.w);
attitude = - PI/2;
bank = 0;
return btVector3(0,0,0);
}
double sqx = q1.x*q1.x;
double sqy = q1.y*q1.y;
double sqz = q1.z*q1.z;
heading = atan2(2*q1.y*q1.w-2*q1.x*q1.z , 1 - 2*sqy - 2*sqz);
attitude = asin(2*test);
bank = atan2(2*q1.x*q1.w-2*q1.y*q1.z , 1 - 2*sqx - 2*sqz);
btVector3 vec(bank,heading,attitude);
return vec;
}