> Euler
I'm using C# and in my program I receive a quaternion of a ridigbody in Quaternions but the axis do not correspond to the orientation of the axis that I use so I want to rotate the quaternion. In order to do this I convert the Quaternion to Euler angles, switch the pitch, yaw and roll so that it correnspond to my coordinate system, convert it back to quaternions and then I generate a rotation matrix to transform positions.
However something goes wrong, When I just convert the same input from quaternion to euler and back I get another quaternion ... so one of the functions does something wrong and I dont know where...
Quaternion q = new Quaternion(-0.4, -0.7, -0.8, 0.5);
double yaw = 0, pitch = 0, roll = 0;
toEuler(q, ref yaw, ref pitch, ref roll);
Quaternion quat = ToQ((float)(yaw), (float)(pitch), (float)(roll));
private void toEuler(Quaternion q, ref double x, ref double y, ref double z)
{
double test = q.X * q.Y + q.Z * q.W;
if (test > 0.499) // singularity at north pole
{
y = 2.0F * System.Math.Atan2(q.X, q.W);
z = Math.PI / 2.0F;
x = 0.0F;
return;
}
if (test < -0.499) // singularity at south pole
{
y = -2.0F * System.Math.Atan2(q.X, q.W);
z = -Math.PI / 2.0F;
x = 0.0F;
return;
}
double sqx = q.X * q.X;
double sqy = q.Y * q.Y;
double sqz = q.Z * q.Z;
y = System.Math.Atan2(2.0F * q.Y * q.W - 2.0 * q.X * q.Z, 1.0F - 2.0 * sqy - 2.0 * sqz);
z = System.Math.Asin(2.0F * test);
x = System.Math.Atan2(2.0 * q.X * q.W - 2.0 * q.Y * q.Z, 1.0F - 2.0 * sqx - 2.0 * sqz);
}
public Quaternion ToQ(float yaw, float pitch, float roll)
{
float rollOver2 = roll * 0.5f;
float sinRollOver2 = (float)Math.Sin((double)rollOver2);
float cosRollOver2 = (float)Math.Cos((double)rollOver2);
float pitchOver2 = pitch * 0.5f;
float sinPitchOver2 = (float)Math.Sin((double)pitchOver2);
float cosPitchOver2 = (float)Math.Cos((double)pitchOver2);
float yawOver2 = yaw * 0.5f;
float sinYawOver2 = (float)Math.Sin((double)yawOver2);
float cosYawOver2 = (float)Math.Cos((double)yawOver2);
Quaternion result = new Quaternion();
result.W = cosYawOver2 * cosPitchOver2 * cosRollOver2 + sinYawOver2 * sinPitchOver2 * sinRollOver2;
result.X = cosYawOver2 * sinPitchOver2 * cosRollOver2 + sinYawOver2 * cosPitchOver2 * sinRollOver2;
result.Y = sinYawOver2 * cosPitchOver2 * cosRollOver2 - cosYawOver2 * sinPitchOver2 * sinRollOver2;
result.Z = cosYawOver2 * cosPitchOver2 * sinRollOver2 - sinYawOver2 * sinPitchOver2 * cosRollOver2;
return result;
}
Your quaternion does not seem normalized. Quaternions representing a rotation should have norm 1.
Be sure that you are using same Euler Angels in convertion "to" and "from".
You can find good reference code from Ken Shoemake http://tog.acm.org/resources/GraphicsGems/gemsiv/euler_angle/
And article "Euler angle conversion" Ken Shoemake from "Graphics Gems" series
To expand on Ben's answer, normalize the quaternion by dividing by the square root of the sum of the squares.
If you need to rotate the quaternion and keep its length, I recommend using quaternions for the rotation. Wikipedia has a great page on this: https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation
链接地址: http://www.djcxy.com/p/81776.html上一篇: 给四元数增加一个欧拉角
下一篇: >欧拉