Vector3 quaternion2Euler(Quaternion q, RotSeq rotSeq)
{
switch (rotSeq)
{
case RotSeq.yxz:
return(threeaxisrot(2 * (q.x * q.z + q.w * q.y),
q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z,
-2 * (q.y * q.z - q.w * q.x),
2 * (q.x * q.y + q.w * q.z),
q.w * q.w - q.x * q.x + q.y * q.y - q.z * q.z));
default:
Debug.LogError("No good sequence");
return(Vector3.zero);
}
}
public static Quaternion FromEuler(Vector3 euler, RotSeq seq)
{
Quaternion result;
var x = Quaternion.Euler(euler.x, 0, 0);
var y = Quaternion.Euler(0, euler.y, 0);
var z = Quaternion.Euler(0, 0, euler.z);
switch (seq)
{
case RotSeq.zyx:
result = z * y * x;
break;
case RotSeq.yzx:
result = y * z * x;
break;
case RotSeq.xzy:
result = x * z * y;
break;
case RotSeq.zxy:
result = z * x * y;
break;
case RotSeq.yxz:
result = y * x * z;
break;
case RotSeq.xyz:
result = x * y * z;
break;
default:
throw new Exception();
}
return(result);
}
static Vector3 _quaternion2Euler(Quaternion q, RotSeq rotSeq)
{
switch (rotSeq)
{
case RotSeq.zyx:
return(threeaxisrot(2 * (q.X * q.Y + q.W * q.Z),
q.W * q.W + q.X * q.X - q.Y * q.Y - q.Z * q.Z,
-2 * (q.X * q.Z - q.W * q.Y),
2 * (q.Y * q.Z + q.W * q.X),
q.W * q.W - q.X * q.X - q.Y * q.Y + q.Z * q.Z));
case RotSeq.zyz:
return(twoaxisrot(2 * (q.Y * q.Z - q.W * q.X),
2 * (q.X * q.Z + q.W * q.Y),
q.W * q.W - q.X * q.X - q.Y * q.Y + q.Z * q.Z,
2 * (q.Y * q.Z + q.W * q.X),
-2 * (q.X * q.Z - q.W * q.Y)));
case RotSeq.zxy:
return(threeaxisrot(-2 * (q.X * q.Y - q.W * q.Z),
q.W * q.W - q.X * q.X + q.Y * q.Y - q.Z * q.Z,
2 * (q.Y * q.Z + q.W * q.X),
-2 * (q.X * q.Z - q.W * q.Y),
q.W * q.W - q.X * q.X - q.Y * q.Y + q.Z * q.Z));
case RotSeq.zxz:
return(twoaxisrot(2 * (q.X * q.Z + q.W * q.Y),
-2 * (q.Y * q.Z - q.W * q.X),
q.W * q.W - q.X * q.X - q.Y * q.Y + q.Z * q.Z,
2 * (q.X * q.Z - q.W * q.Y),
2 * (q.Y * q.Z + q.W * q.X)));
case RotSeq.yxz:
return(threeaxisrot(2 * (q.X * q.Z + q.W * q.Y),
q.W * q.W - q.X * q.X - q.Y * q.Y + q.Z * q.Z,
-2 * (q.Y * q.Z - q.W * q.X),
2 * (q.X * q.Y + q.W * q.Z),
q.W * q.W - q.X * q.X + q.Y * q.Y - q.Z * q.Z));
case RotSeq.yxy:
return(twoaxisrot(2 * (q.X * q.Y - q.W * q.Z),
2 * (q.Y * q.Z + q.W * q.X),
q.W * q.W - q.X * q.X + q.Y * q.Y - q.Z * q.Z,
2 * (q.X * q.Y + q.W * q.Z),
-2 * (q.Y * q.Z - q.W * q.X)));
case RotSeq.yzx:
return(threeaxisrot(-2 * (q.X * q.Z - q.W * q.Y),
q.W * q.W + q.X * q.X - q.Y * q.Y - q.Z * q.Z,
2 * (q.X * q.Y + q.W * q.Z),
-2 * (q.Y * q.Z - q.W * q.X),
q.W * q.W - q.X * q.X + q.Y * q.Y - q.Z * q.Z));
case RotSeq.yzy:
return(twoaxisrot(2 * (q.Y * q.Z + q.W * q.X),
-2 * (q.X * q.Y - q.W * q.Z),
q.W * q.W - q.X * q.X + q.Y * q.Y - q.Z * q.Z,
2 * (q.Y * q.Z - q.W * q.X),
2 * (q.X * q.Y + q.W * q.Z)));
case RotSeq.xyz:
return(threeaxisrot(-2 * (q.Y * q.Z - q.W * q.X),
q.W * q.W - q.X * q.X - q.Y * q.Y + q.Z * q.Z,
2 * (q.X * q.Z + q.W * q.Y),
-2 * (q.X * q.Y - q.W * q.Z),
q.W * q.W + q.X * q.X - q.Y * q.Y - q.Z * q.Z));
case RotSeq.xyx:
return(twoaxisrot(2 * (q.X * q.Y + q.W * q.Z),
-2 * (q.X * q.Z - q.W * q.Y),
q.W * q.W + q.X * q.X - q.Y * q.Y - q.Z * q.Z,
2 * (q.X * q.Y - q.W * q.Z),
2 * (q.X * q.Z + q.W * q.Y)));
case RotSeq.xzy:
return(threeaxisrot(2 * (q.Y * q.Z - q.W * q.X),
q.W * q.W - q.X * q.X + q.Y * q.Y - q.Z * q.Z,
-2 * (q.X * q.Y + q.W * q.Z),
2 * (q.X * q.Z - q.W * q.Y),
q.W * q.W + q.X * q.X - q.Y * q.Y - q.Z * q.Z));
case RotSeq.xzx:
return(twoaxisrot(2 * (q.X * q.Z - q.W * q.Y),
2 * (q.X * q.Y + q.W * q.Z),
q.W * q.W + q.X * q.X - q.Y * q.Y - q.Z * q.Z,
2 * (q.X * q.Z + q.W * q.Y),
-2 * (q.X * q.Y - q.W * q.Z)));
default:
return(Vector3.Zero);
}
}
public static System.Numerics.Vector3 quaternion2Euler(Quaternion q, RotSeq rotSeq)
{
Vector3 res = _quaternion2Euler(q, rotSeq);
var result = new System.Numerics.Vector3();
float test = q.W * q.Z + q.X * q.Y;
float unit = q.X * q.X + q.Y * q.Y + q.Z * q.Z + q.W * q.W;
switch (rotSeq)
{
case RotSeq.zyx:
result.X = res.X;
result.Y = res.Y;
result.Z = res.Z;
break;
case RotSeq.zxy:
result.X = res.Y;
result.Y = res.X;
result.Z = res.Z;
break;
case RotSeq.yxz:
result.X = res.Z;
result.Y = res.X;
result.Z = res.Y;
break;
case RotSeq.yzx:
result.X = res.X;
result.Y = res.Z;
result.Z = res.Y;
if (test > 0.4995f * unit)
{
result.X = 0.0f;
result.Y = 2.0f * (float)Math.Atan2(q.Y, q.Z);
result.Z = 90.0f * Utils.Deg2Rad;
}
if (test < -0.4995f * unit)
{
result.X = 0.0f;
result.Y = -2.0f * (float)Math.Atan2(q.Y, q.Z);
result.Z = -90.0f * Utils.Deg2Rad;
}
break;
case RotSeq.xyz:
result.X = res.Z;
result.Y = res.Y;
result.Z = res.X;
break;
case RotSeq.xzy:
result.X = -res.Z;
result.Y = -res.X;
result.Z = -res.Y;
// Handle poles
test = q.Y * q.Z - q.W * q.X;
if (test > 0.4995f * unit)
{
result.Y = 0.0f;
result.X = 2.0f * (float)Math.Atan2(q.X, q.Z);
result.Z = 90.0f * Utils.Deg2Rad;
}
if (test < -0.4995f * unit)
{
result.Y = 0.0f;
result.X = -2.0f * (float)Math.Atan2(q.X, q.Z);
result.Z = -90.0f * Utils.Deg2Rad;
}
break;
default:
return(System.Numerics.Vector3.Zero);
}
result.X = (result.X <= -180.0f * Utils.Deg2Rad) ? result.X + 360.0f * Utils.Deg2Rad : result.X;
result.Y = (result.Y <= -180.0f * Utils.Deg2Rad) ? result.Y + 360.0f * Utils.Deg2Rad : result.Y;
result.Z = (result.Z <= -180.0f * Utils.Deg2Rad) ? result.Z + 360.0f * Utils.Deg2Rad : result.Z;
return(result);
}
void quaternion2Euler(Quaternion q, ref Vector3 res, RotSeq rotSeq)
{
switch (rotSeq)
{
case RotSeq.zyx:
threeaxisrot(2 * (q.x * q.y + q.w * q.z),
q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z,
-2 * (q.x * q.z - q.w * q.y),
2 * (q.y * q.z + q.w * q.x),
q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z,
ref res);
break;
case RotSeq.zyz:
twoaxisrot(2 * (q.y * q.z - q.w * q.x),
2 * (q.x * q.z + q.w * q.y),
q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z,
2 * (q.y * q.z + q.w * q.x),
-2 * (q.x * q.z - q.w * q.y),
ref res);
break;
case RotSeq.zxy:
threeaxisrot(-2 * (q.x * q.y - q.w * q.z),
q.w * q.w - q.x * q.x + q.y * q.y - q.z * q.z,
2 * (q.y * q.z + q.w * q.x),
-2 * (q.x * q.z - q.w * q.y),
q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z,
ref res);
break;
case RotSeq.zxz:
twoaxisrot(2 * (q.x * q.z + q.w * q.y),
-2 * (q.y * q.z - q.w * q.x),
q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z,
2 * (q.x * q.z - q.w * q.y),
2 * (q.y * q.z + q.w * q.x),
ref res);
break;
case RotSeq.yxz:
threeaxisrot(2 * (q.x * q.z + q.w * q.y),
q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z,
-2 * (q.y * q.z - q.w * q.x),
2 * (q.x * q.y + q.w * q.z),
q.w * q.w - q.x * q.x + q.y * q.y - q.z * q.z,
ref res);
break;
case RotSeq.yxy:
twoaxisrot(2 * (q.x * q.y - q.w * q.z),
2 * (q.y * q.z + q.w * q.x),
q.w * q.w - q.x * q.x + q.y * q.y - q.z * q.z,
2 * (q.x * q.y + q.w * q.z),
-2 * (q.y * q.z - q.w * q.x),
ref res);
break;
case RotSeq.yzx:
threeaxisrot(-2 * (q.x * q.z - q.w * q.y),
q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z,
2 * (q.x * q.y + q.w * q.z),
-2 * (q.y * q.z - q.w * q.x),
q.w * q.w - q.x * q.x + q.y * q.y - q.z * q.z,
ref res);
break;
case RotSeq.yzy:
twoaxisrot(2 * (q.y * q.z + q.w * q.x),
-2 * (q.x * q.y - q.w * q.z),
q.w * q.w - q.x * q.x + q.y * q.y - q.z * q.z,
2 * (q.y * q.z - q.w * q.x),
2 * (q.x * q.y + q.w * q.z),
ref res);
break;
case RotSeq.xyz:
threeaxisrot(-2 * (q.y * q.z - q.w * q.x),
q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z,
2 * (q.x * q.z + q.w * q.y),
-2 * (q.x * q.y - q.w * q.z),
q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z,
ref res);
break;
case RotSeq.xyx:
twoaxisrot(2 * (q.x * q.y + q.w * q.z),
-2 * (q.x * q.z - q.w * q.y),
q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z,
2 * (q.x * q.y - q.w * q.z),
2 * (q.x * q.z + q.w * q.y),
ref res);
break;
case RotSeq.xzy:
threeaxisrot(2 * (q.y * q.z + q.w * q.x),
q.w * q.w - q.x * q.x + q.y * q.y - q.z * q.z,
-2 * (q.x * q.y - q.w * q.z),
2 * (q.x * q.z + q.w * q.y),
q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z,
ref res);
break;
case RotSeq.xzx:
twoaxisrot(2 * (q.x * q.z - q.w * q.y),
2 * (q.x * q.y + q.w * q.z),
q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z,
2 * (q.x * q.z + q.w * q.y),
-2 * (q.x * q.y - q.w * q.z),
ref res);
break;
default:
break;
}
res.x = Mathf.Rad2Deg * (res.x);
res.y = Mathf.Rad2Deg * (res.y);
res.z = Mathf.Rad2Deg * (res.z);
}
private static Vector3 quaternion2Euler(Quaternion q, RotSeq rotSeq)
{
switch (rotSeq)
{
case RotSeq.zyx:
return(Threeaxisrot(2 * (q.x * q.y + q.w * q.z),
q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z,
-2 * (q.x * q.z - q.w * q.y),
2 * (q.y * q.z + q.w * q.x),
q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z));
case RotSeq.zyz:
return(Twoaxisrot(2 * (q.y * q.z - q.w * q.x),
2 * (q.x * q.z + q.w * q.y),
q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z,
2 * (q.y * q.z + q.w * q.x),
-2 * (q.x * q.z - q.w * q.y)));
case RotSeq.zxy:
return(Threeaxisrot(-2 * (q.x * q.y - q.w * q.z),
q.w * q.w - q.x * q.x + q.y * q.y - q.z * q.z,
2 * (q.y * q.z + q.w * q.x),
-2 * (q.x * q.z - q.w * q.y),
q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z));
case RotSeq.zxz:
return(Twoaxisrot(2 * (q.x * q.z + q.w * q.y),
-2 * (q.y * q.z - q.w * q.x),
q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z,
2 * (q.x * q.z - q.w * q.y),
2 * (q.y * q.z + q.w * q.x)));
case RotSeq.yxz:
return(Threeaxisrot(2 * (q.x * q.z + q.w * q.y),
q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z,
-2 * (q.y * q.z - q.w * q.x),
2 * (q.x * q.y + q.w * q.z),
q.w * q.w - q.x * q.x + q.y * q.y - q.z * q.z));
case RotSeq.yxy:
return(Twoaxisrot(2 * (q.x * q.y - q.w * q.z),
2 * (q.y * q.z + q.w * q.x),
q.w * q.w - q.x * q.x + q.y * q.y - q.z * q.z,
2 * (q.x * q.y + q.w * q.z),
-2 * (q.y * q.z - q.w * q.x)));
case RotSeq.yzx:
return(Threeaxisrot(-2 * (q.x * q.z - q.w * q.y),
q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z,
2 * (q.x * q.y + q.w * q.z),
-2 * (q.y * q.z - q.w * q.x),
q.w * q.w - q.x * q.x + q.y * q.y - q.z * q.z));
case RotSeq.yzy:
return(Twoaxisrot(2 * (q.y * q.z + q.w * q.x),
-2 * (q.x * q.y - q.w * q.z),
q.w * q.w - q.x * q.x + q.y * q.y - q.z * q.z,
2 * (q.y * q.z - q.w * q.x),
2 * (q.x * q.y + q.w * q.z)));
case RotSeq.xyz:
return(Threeaxisrot(-2 * (q.y * q.z - q.w * q.x),
q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z,
2 * (q.x * q.z + q.w * q.y),
-2 * (q.x * q.y - q.w * q.z),
q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z));
case RotSeq.xyx:
return(Twoaxisrot(2 * (q.x * q.y + q.w * q.z),
-2 * (q.x * q.z - q.w * q.y),
q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z,
2 * (q.x * q.y - q.w * q.z),
2 * (q.x * q.z + q.w * q.y)));
case RotSeq.xzy:
return(Threeaxisrot(2 * (q.y * q.z + q.w * q.x),
q.w * q.w - q.x * q.x + q.y * q.y - q.z * q.z,
-2 * (q.x * q.y - q.w * q.z),
2 * (q.x * q.z + q.w * q.y),
q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z));
case RotSeq.xzx:
return(Twoaxisrot(2 * (q.x * q.z - q.w * q.y),
2 * (q.x * q.y + q.w * q.z),
q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z,
2 * (q.x * q.z + q.w * q.y),
-2 * (q.x * q.y - q.w * q.z)));
default:
Debug.LogError("No good sequence");
return(Vector3.zero);
}
}
void quaternion2Euler(Quaternion q, out Vector3 ypr, RotSeq rotSeq)
{
Vector3 res = new Vector3(0);
switch (rotSeq)
{
case RotSeq.zyx:
threeaxisrot(2 * (q.X * q.Y + q.W * q.Z),
q.W * q.W + q.X * q.X - q.Y * q.Y - q.Z * q.Z,
-2 * (q.X * q.Z - q.W * q.Y),
2 * (q.Y * q.Z + q.W * q.X),
q.W * q.W - q.X * q.X - q.Y * q.Y + q.Z * q.Z,
out res);
break;
case RotSeq.zxy:
threeaxisrot(-2 * (q.X * q.Y - q.W * q.Z),
q.W * q.W - q.X * q.X + q.Y * q.Y - q.Z * q.Z,
2 * (q.Y * q.Z + q.W * q.X),
-2 * (q.X * q.Z - q.W * q.Y),
q.W * q.W - q.X * q.X - q.Y * q.Y + q.Z * q.Z,
out res);
break;
case RotSeq.yxz: //то что нужно!
threeaxisrot(2 * (q.X * q.Z + q.W * q.Y),
q.W * q.W - q.X * q.X - q.Y * q.Y + q.Z * q.Z,
-2 * (q.Y * q.Z - q.W * q.X),
2 * (q.X * q.Y + q.W * q.Z),
q.W * q.W - q.X * q.X + q.Y * q.Y - q.Z * q.Z,
out res);
break;
case RotSeq.yzx:
threeaxisrot(-2 * (q.X * q.Z - q.W * q.Y),
q.W * q.W + q.X * q.X - q.Y * q.Y - q.Z * q.Z,
2 * (q.X * q.Y + q.W * q.Z),
-2 * (q.Y * q.Z - q.W * q.X),
q.W * q.W - q.X * q.X + q.Y * q.Y - q.Z * q.Z,
out res);
break;
case RotSeq.xyz:
threeaxisrot(-2 * (q.Y * q.Z - q.W * q.X),
q.W * q.W - q.X * q.X - q.Y * q.Y + q.Z * q.Z,
2 * (q.X * q.Z + q.W * q.Y),
-2 * (q.X * q.Y - q.W * q.Z),
q.W * q.W + q.X * q.X - q.Y * q.Y - q.Z * q.Z,
out res);
break;
case RotSeq.xzy:
threeaxisrot(2 * (q.Y * q.Z + q.W * q.X),
q.W * q.W - q.X * q.X + q.Y * q.Y - q.Z * q.Z,
-2 * (q.X * q.Y - q.W * q.Z),
2 * (q.X * q.Z + q.W * q.Y),
q.W * q.W + q.X * q.X - q.Y * q.Y - q.Z * q.Z,
out res);
break;
default:
break;
}
ypr = res;
}
// note:
// return values of res[] depends on rotSeq.
// i.e.
// for rotSeq zyx,
// x = res[0], y = res[1], z = res[2]
// for rotSeq xyz
// z = res[0], y = res[1], x = res[2]
// ...
public static Vector3 ToEuler(Quaternion q, RotSeq rotSeq)
{
q = normalize(q);
var v = new float[3];
switch (rotSeq)
{
case RotSeq.zyx:
threeaxisrot(2 * (q.x * q.y + q.w * q.z),
q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z,
-2 * (q.x * q.z - q.w * q.y),
2 * (q.y * q.z + q.w * q.x),
q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z,
v);
break;
case RotSeq.zyz:
twoaxisrot(2 * (q.y * q.z - q.w * q.x),
2 * (q.x * q.z + q.w * q.y),
q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z,
2 * (q.y * q.z + q.w * q.x),
-2 * (q.x * q.z - q.w * q.y),
v);
break;
case RotSeq.zxy:
threeaxisrot(-2 * (q.x * q.y - q.w * q.z),
q.w * q.w - q.x * q.x + q.y * q.y - q.z * q.z,
2 * (q.y * q.z + q.w * q.x),
-2 * (q.x * q.z - q.w * q.y),
q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z,
v);
break;
case RotSeq.zxz:
twoaxisrot(2 * (q.x * q.z + q.w * q.y),
-2 * (q.y * q.z - q.w * q.x),
q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z,
2 * (q.x * q.z - q.w * q.y),
2 * (q.y * q.z + q.w * q.x),
v);
break;
case RotSeq.yxz:
threeaxisrot(2 * (q.x * q.z + q.w * q.y),
q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z,
-2 * (q.y * q.z - q.w * q.x),
2 * (q.x * q.y + q.w * q.z),
q.w * q.w - q.x * q.x + q.y * q.y - q.z * q.z,
v);
break;
case RotSeq.yxy:
twoaxisrot(2 * (q.x * q.y - q.w * q.z),
2 * (q.y * q.z + q.w * q.x),
q.w * q.w - q.x * q.x + q.y * q.y - q.z * q.z,
2 * (q.x * q.y + q.w * q.z),
-2 * (q.y * q.z - q.w * q.x),
v);
break;
case RotSeq.yzx:
threeaxisrot(-2 * (q.x * q.z - q.w * q.y),
q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z,
2 * (q.x * q.y + q.w * q.z),
-2 * (q.y * q.z - q.w * q.x),
q.w * q.w - q.x * q.x + q.y * q.y - q.z * q.z,
v);
break;
case RotSeq.yzy:
twoaxisrot(2 * (q.y * q.z + q.w * q.x),
-2 * (q.x * q.y - q.w * q.z),
q.w * q.w - q.x * q.x + q.y * q.y - q.z * q.z,
2 * (q.y * q.z - q.w * q.x),
2 * (q.x * q.y + q.w * q.z),
v);
break;
case RotSeq.xyz:
threeaxisrot(-2 * (q.y * q.z - q.w * q.x),
q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z,
2 * (q.x * q.z + q.w * q.y),
-2 * (q.x * q.y - q.w * q.z),
q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z,
v);
break;
case RotSeq.xyx:
twoaxisrot(2 * (q.x * q.y + q.w * q.z),
-2 * (q.x * q.z - q.w * q.y),
q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z,
2 * (q.x * q.y - q.w * q.z),
2 * (q.x * q.z + q.w * q.y),
v);
break;
case RotSeq.xzy:
threeaxisrot(2 * (q.y * q.z + q.w * q.x),
q.w * q.w - q.x * q.x + q.y * q.y - q.z * q.z,
-2 * (q.x * q.y - q.w * q.z),
2 * (q.x * q.z + q.w * q.y),
q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z,
v);
break;
case RotSeq.xzx:
twoaxisrot(2 * (q.x * q.z - q.w * q.y),
2 * (q.x * q.y + q.w * q.z),
q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z,
2 * (q.x * q.z + q.w * q.y),
-2 * (q.x * q.y - q.w * q.z),
v);
break;
default:
throw new Exception();
}
return(new Vector3(v[0], v[1], v[2]) * Mathf.Rad2Deg);
}
public static System.Numerics.Vector3 quaternion2EulerDeg(Quaternion q, RotSeq rotSeq)
{
Vector3 res = quaternion2Euler(q, rotSeq);
var result = new System.Numerics.Vector3();
float test = q.w * q.z + q.x * q.y;
float unit = q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w;
switch (rotSeq)
{
case RotSeq.zyx:
result.X = res.x * Mathf.Rad2Deg;
result.Y = res.y * Mathf.Rad2Deg;
result.Z = res.z * Mathf.Rad2Deg;
break;
case RotSeq.zxy:
result.X = res.y * Mathf.Rad2Deg;
result.Y = res.x * Mathf.Rad2Deg;
result.Z = res.z * Mathf.Rad2Deg;
break;
case RotSeq.yxz:
result.X = res.z * Mathf.Rad2Deg;
result.Y = res.x * Mathf.Rad2Deg;
result.Z = res.y * Mathf.Rad2Deg;
break;
case RotSeq.yzx:
result.X = res.x * Mathf.Rad2Deg;
result.Y = res.z * Mathf.Rad2Deg;
result.Z = res.y * Mathf.Rad2Deg;
// Handle poles
if (test > 0.4995f * unit)
{
result.X = 0.0f;
result.Y = 2.0f * Mathf.Atan2(q.y, q.z) * Mathf.Rad2Deg;
result.Z = 90.0f;
}
if (test < -0.4995f * unit)
{
result.X = 0.0f;
result.Y = -2.0f * Mathf.Atan2(q.y, q.z) * Mathf.Rad2Deg;
result.Z = -90.0f;
}
break;
case RotSeq.xyz:
result.X = res.z * Mathf.Rad2Deg;
result.Y = res.y * Mathf.Rad2Deg;
result.Z = res.x * Mathf.Rad2Deg;
break;
case RotSeq.xzy:
result.X = res.y * Mathf.Rad2Deg;
result.Y = res.z * Mathf.Rad2Deg;
result.Z = res.x * Mathf.Rad2Deg;
break;
default:
Debug.LogError("No good sequence");
return(System.Numerics.Vector3.Zero);
}
result.X = (result.X <= -180.0f) ? result.X + 360.0f : result.X;
result.Y = (result.Y <= -180.0f) ? result.Y + 360.0f : result.Y;
result.Z = (result.Z <= -180.0f) ? result.Z + 360.0f : result.Z;
return(result);
}
