/// <summary>
/// Creates a quaternionr from the Euler rotations specified
/// </summary>
/// <param name="quaternion"></param>
/// <param name="x">X rotation in radians</param>
/// <param name="y">Y rotation in radians</param>
/// <param name="z">Z rotation in radians</param>
/// <param name="order"></param>
/// <returns></returns>
public static float[] QuaternionFromEuler(this float[] euler, EulerOrder order)
{
// http://www.mathworks.com/matlabcentral/fileexchange/
// 20696-function-to-convert-between-dcm-euler-angles-quaternions-and-euler-vectors/
// content/SpinCalc.m
var c1 = (float)Math.Cos(euler[0] / 2);
var c2 = (float)Math.Cos(euler[1] / 2);
var c3 = (float)Math.Cos(euler[2] / 2);
var s1 = (float)Math.Sin(euler[0] / 2);
var s2 = (float)Math.Sin(euler[1] / 2);
var s3 = (float)Math.Sin(euler[2] / 2);
var result = new float[4];
if (order == EulerOrder.XYZ)
{
result[0] = s1 * c2 * c3 + c1 * s2 * s3;
result[1] = c1 * s2 * c3 - s1 * c2 * s3;
result[2] = c1 * c2 * s3 + s1 * s2 * c3;
result[3] = c1 * c2 * c3 - s1 * s2 * s3;
}
else if (order == EulerOrder.YXZ)
{
result[0] = s1 * c2 * c3 + c1 * s2 * s3;
result[1] = c1 * s2 * c3 - s1 * c2 * s3;
result[2] = c1 * c2 * s3 - s1 * s2 * c3;
result[3] = c1 * c2 * c3 + s1 * s2 * s3;
}
else if (order == EulerOrder.ZXY)
{
result[0] = s1 * c2 * c3 - c1 * s2 * s3;
result[1] = c1 * s2 * c3 + s1 * c2 * s3;
result[2] = c1 * c2 * s3 + s1 * s2 * c3;
result[3] = c1 * c2 * c3 - s1 * s2 * s3;
}
else if (order == EulerOrder.ZYX)
{
result[0] = s1 * c2 * c3 - c1 * s2 * s3;
result[1] = c1 * s2 * c3 + s1 * c2 * s3;
result[2] = c1 * c2 * s3 - s1 * s2 * c3;
result[3] = c1 * c2 * c3 + s1 * s2 * s3;
}
else if (order == EulerOrder.YZX)
{
result[0] = s1 * c2 * c3 + c1 * s2 * s3;
result[1] = c1 * s2 * c3 + s1 * c2 * s3;
result[2] = c1 * c2 * s3 - s1 * s2 * c3;
result[3] = c1 * c2 * c3 - s1 * s2 * s3;
}
else if (order == EulerOrder.XZY)
{
result[0] = s1 * c2 * c3 - c1 * s2 * s3;
result[1] = c1 * s2 * c3 - s1 * c2 * s3;
result[2] = c1 * c2 * s3 + s1 * s2 * c3;
result[3] = c1 * c2 * c3 + s1 * s2 * s3;
}
return(result);
}
public static void GetEulerAngles(ref Matrix matrix, EulerOrder order, RotationDirection rotation, Handedness handedness, ref Vector3D eulerAngles)
{
GetEulerAngles(ref matrix, (long)order, (long)rotation, (long)handedness, ref eulerAngles);
eulerAngles.x *= Mathf.Rad2Deg;
eulerAngles.y *= Mathf.Rad2Deg;
eulerAngles.z *= Mathf.Rad2Deg;
}
public static Vector3 switchAxis(Vector3 r, EulerOrder order)
{
switch (order)
{
case EulerOrder.XYZ:
return(new Vector3(r.x, r.y, r.z));
case EulerOrder.XZY:
return(new Vector3(r.x, r.z, r.y));
case EulerOrder.YXZ:
return(new Vector3(r.y, r.x, r.z));
case EulerOrder.YZX:
return(new Vector3(r.y, r.z, r.x));
case EulerOrder.ZXY:
return(new Vector3(r.z, r.x, r.y));
case EulerOrder.ZYX:
return(new Vector3(r.z, r.y, r.x));
default:
return(r);
}
}
public Coordinate(Coordinate c)
{
up = c.up;
forward = c.forward;
right = c.right;
origin = c.origin;
originPos = c.originPos;
order = c.order;
}
public void Rotate(float x, float y, float z, EulerOrder order = EulerOrder.XYZ)
{
double dx = x * Math.PI / 180f;
double dy = y * Math.PI / 180f;
double dz = z * Math.PI / 180f;
float
sinA = (float)Math.Sin(dx),
cosA = (float)Math.Cos(dx),
sinB = (float)Math.Sin(dy),
cosB = (float)Math.Cos(dy),
sinC = (float)Math.Sin(dz),
cosC = (float)Math.Cos(dz);
float m2a1, m2b1, m2c1,
m2a2, m2b2, m2c2,
m2a3, m2b3, m2c3;
switch (order)
{
default:
case EulerOrder.XYZ:
m2a1 = cosB * cosC;
m2b1 = -cosB * sinC;
m2c1 = sinB;
m2a2 = cosC * sinA * sinB + cosA * sinC;
m2b2 = -sinC * sinB * sinA + cosC * cosA;
m2c2 = -cosB * sinA;
m2a3 = -cosC * sinB * cosA + sinC * sinA;
m2b3 = cosC * sinA + sinC * sinB * cosA;
m2c3 = cosB * cosA;
break;
case EulerOrder.ZYX:
m2a1 = cosC * cosB;
m2b1 = -sinC * cosA + cosC * sinB * sinA;
m2c1 = sinC * sinA + cosC * sinB * cosA;
m2a2 = sinC * cosB;
m2b2 = cosC * cosA + sinC * sinB * sinA;
m2c2 = -cosC * sinA + sinC * sinB * cosA;
m2a3 = -sinB;
m2b3 = cosB * sinA;
m2c3 = cosB * cosA;
break;
case EulerOrder.XZY:
case EulerOrder.YXZ:
case EulerOrder.YZX:
case EulerOrder.ZXY:
throw new NotSupportedException("Specified euler order is not supported yet.");
}
float a1 = this.a1 * m2a1 + this.b1 * m2a2 + this.c1 * m2a3;
float b1 = this.a1 * m2b1 + this.b1 * m2b2 + this.c1 * m2b3;
float c1 = this.a1 * m2c1 + this.b1 * m2c2 + this.c1 * m2c3;
float a2 = this.a2 * m2a1 + this.b2 * m2a2 + this.c2 * m2a3;
float b2 = this.a2 * m2b1 + this.b2 * m2b2 + this.c2 * m2b3;
float c2 = this.a2 * m2c1 + this.b2 * m2c2 + this.c2 * m2c3;
float a3 = this.a3 * m2a1 + this.b3 * m2a2 + this.c3 * m2a3;
float b3 = this.a3 * m2b1 + this.b3 * m2b2 + this.c3 * m2b3;
float c3 = this.a3 * m2c1 + this.b3 * m2c2 + this.c3 * m2c3;
float a4 = this.a4 * m2a1 + this.b4 * m2a2 + this.c4 * m2a3;
float b4 = this.a4 * m2b1 + this.b4 * m2b2 + this.c4 * m2b3;
float c4 = this.a4 * m2c1 + this.b4 * m2c2 + this.c4 * m2c3;
this.a1 = a1; this.b1 = b1; this.c1 = c1;
this.a2 = a2; this.b2 = b2; this.c2 = c2;
this.a3 = a3; this.b3 = b3; this.c3 = c3;
this.a4 = a4; this.b4 = b4; this.c4 = c4;
}
public void Rotate(Vector3 v, EulerOrder order = EulerOrder.XYZ)
{
this.Rotate(v.x, v.y, v.z, order);
}
// Query representations
/**
@brief get rotation in Euler angles representation.
Euler angles are a 3D point that represents rotation in 3D. Each variable is the angle
of rotation around a certain axis (x/y/z).
@param[in] order the order in which we get the Euler angles (ROLL_PITCH_YAW as default)
@return 3D point containing Euler angles (RADIANS) in the given order.
*/
public PXCMPoint3DF32 QueryEulerAngles(EulerOrder order) {
return PXCMRotation_QueryEulerAngles(instance, order);
}
internal extern static PXCMPoint3DF32 PXCMRotation_QueryEulerAngles(IntPtr instance, EulerOrder order);
public static NVector3 QuaternionToEuler(Quaternion q, EulerOrder rotSeq)
{
NVector3 res = _quaternion2Euler(q, rotSeq);
var result = new NVector3();
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 EulerOrder.ZYX:
result.X = res.X;
result.Y = res.Y;
result.Z = res.Z;
break;
case EulerOrder.ZXY:
result.X = res.Y;
result.Y = res.X;
result.Z = res.Z;
break;
case EulerOrder.YXZ:
result.X = res.Z;
result.Y = res.X;
result.Z = res.Y;
break;
case EulerOrder.YZX:
result.X = res.X;
result.Y = res.Z;
result.Z = res.Y;
// Handle poles
if (test > 0.4995f * unit)
{
result.X = 0.0f;
result.Y = 2.0f * (float)Math.Atan2(q.Y, q.Z);
result.Z = 90.0f * 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 * Deg2Rad;
}
break;
case EulerOrder.XYZ:
result.X = res.Z;
result.Y = res.Y;
result.Z = res.X;
break;
case EulerOrder.XZY:
result.X = res.Y;
result.Y = res.Z;
result.Z = res.X;
// Handle poles
if (test > 0.4995f * unit)
{
result.X = -90.0f * Deg2Rad;
result.Y = -2.0f * (float)Math.Atan2(q.Y, q.Z);
result.Z = 0;
}
if (test < -0.4995f * unit)
{
result.X = 0.0f;
result.Y = 2.0f * (float)Math.Atan2(q.Y, q.Z);
result.Z = -90.0f * Deg2Rad;
}
break;
default:
return(System.Numerics.Vector3.Zero);
}
result.X = (result.X <= -180.0f * Deg2Rad) ? result.X + 360.0f * Deg2Rad : result.X;
result.Y = (result.Y <= -180.0f * Deg2Rad) ? result.Y + 360.0f * Deg2Rad : result.Y;
result.Z = (result.Z <= -180.0f * Deg2Rad) ? result.Z + 360.0f * Deg2Rad : result.Z;
return(result);
}
static NVector3 _quaternion2Euler(Quaternion q, EulerOrder rotSeq)
{
switch (rotSeq)
{
case EulerOrder.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 EulerOrder.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 EulerOrder.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 EulerOrder.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 EulerOrder.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 EulerOrder.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 EulerOrder.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 EulerOrder.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 EulerOrder.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 EulerOrder.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 EulerOrder.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 EulerOrder.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(NVector3.Zero);
}
}
// Query representations
/**
* @brief get rotation in Euler angles representation.
* Euler angles are a 3D point that represents rotation in 3D. Each variable is the angle
* of rotation around a certain axis (x/y/z).
*
* @param[in] order the order in which we get the Euler angles (ROLL_PITCH_YAW as default)
* @return 3D point containing Euler angles (RADIANS) in the given order.
*/
public PXCMPoint3DF32 QueryEulerAngles(EulerOrder order)
{
return(PXCMRotation_QueryEulerAngles(instance, order));
}
public static float[] EulerFromMatrix(this float[] matrix, EulerOrder order)
{
var result = new float[] { 0, 0, 0 };
// assumes the upper 3x3 of m is a pure rotation matrix (i.e, unscaled)
var m11 = matrix[0]; var m12 = matrix[4]; var m13 = matrix[8];
var m21 = matrix[1]; var m22 = matrix[5]; var m23 = matrix[9];
var m31 = matrix[2]; var m32 = matrix[6]; var m33 = matrix[10];
if (order == EulerOrder.XYZ)
{
result[1] = (float)Math.Asin(m13.Clamp(-1, 1));
if (Math.Abs(m13) < 0.99999)
{
result[0] = (float)Math.Atan2(-m23, m33);
result[2] = (float)Math.Atan2(-m12, m11);
}
else
{
result[0] = (float)Math.Atan2(m32, m22);
result[2] = 0;
}
}
else if (order == EulerOrder.YXZ)
{
result[0] = (float)Math.Asin(-(m23.Clamp(-1, 1)));
if (Math.Abs(m23) < 0.99999)
{
result[1] = (float)Math.Atan2(m13, m33);
result[2] = (float)Math.Atan2(m21, m22);
}
else
{
result[1] = (float)Math.Atan2(-m31, m11);
result[2] = 0;
}
}
else if (order == EulerOrder.ZXY)
{
result[0] = (float)Math.Asin(m32.Clamp(-1, 1));
if (Math.Abs(m32) < 0.99999)
{
result[1] = (float)Math.Atan2(-m31, m33);
result[2] = (float)Math.Atan2(-m12, m22);
}
else
{
result[1] = 0;
result[2] = (float)Math.Atan2(m21, m11);
}
}
else if (order == EulerOrder.ZYX)
{
result[1] = (float)Math.Asin(-(m31.Clamp(-1, 1)));
if (Math.Abs(m31) < 0.99999)
{
result[0] = (float)Math.Atan2(m32, m33);
result[2] = (float)Math.Atan2(m21, m11);
}
else
{
result[0] = 0;
result[2] = (float)Math.Atan2(-m12, m22);
}
}
else if (order == EulerOrder.YZX)
{
result[2] = (float)Math.Asin(m21.Clamp(-1, 1));
if (Math.Abs(m21) < 0.99999)
{
result[0] = (float)Math.Atan2(-m23, m22);
result[1] = (float)Math.Atan2(-m31, m11);
}
else
{
result[0] = 0;
result[1] = (float)Math.Atan2(m13, m33);
}
}
else if (order == EulerOrder.XZY)
{
result[2] = (float)Math.Asin(-(m12.Clamp(-1, 1)));
if (Math.Abs(m12) < 0.99999)
{
result[0] = (float)Math.Atan2(m32, m22);
result[1] = (float)Math.Atan2(m13, m11);
}
else
{
result[0] = (float)Math.Atan2(-m23, m33);
result[1] = 0;
}
}
return(result);
}
public static float[] EulerFromQuaternion(this float[] quaternion, EulerOrder order)
{
var matrix = quaternion.MatrixFromQuaternion();
return(matrix.EulerFromMatrix(order));
}
public static float[] MatrixFromEuler(this float[] euler, EulerOrder order)
{
var result = new float[16];
var x = euler[0];
var y = euler[1];
var z = euler[2];
var a = (float)Math.Cos(x);
var b = (float)Math.Sin(x);
var c = (float)Math.Cos(y);
var d = (float)Math.Sin(y);
var e = (float)Math.Cos(z);
var f = (float)Math.Sin(z);
if (order == EulerOrder.XYZ)
{
var ae = a * e;
var af = a * f;
var be = b * e;
var bf = b * f;
result[0] = c * e;
result[4] = -c * f;
result[8] = d;
result[1] = af + be * d;
result[5] = ae - bf * d;
result[9] = -b * c;
result[2] = bf - ae * d;
result[6] = be + af * d;
result[10] = a * c;
}
else if (order == EulerOrder.YXZ)
{
var ce = c * e;
var cf = c * f;
var de = d * e;
var df = d * f;
result[0] = ce + df * b;
result[4] = de * b - cf;
result[8] = a * d;
result[1] = a * f;
result[5] = a * e;
result[9] = -b;
result[2] = cf * b - de;
result[6] = df + ce * b;
result[10] = a * c;
}
else if (order == EulerOrder.ZXY)
{
var ce = c * e;
var cf = c * f;
var de = d * e;
var df = d * f;
result[0] = ce - df * b;
result[4] = -a * f;
result[8] = de + cf * b;
result[1] = cf + de * b;
result[5] = a * e;
result[9] = df - ce * b;
result[2] = -a * d;
result[6] = b;
result[10] = a * c;
}
else if (order == EulerOrder.ZYX)
{
var ae = a * e;
var af = a * f;
var be = b * e;
var bf = b * f;
result[0] = c * e;
result[4] = be * d - af;
result[8] = ae * d + bf;
result[1] = c * f;
result[5] = bf * d + ae;
result[9] = af * d - be;
result[2] = -d;
result[6] = b * c;
result[10] = a * c;
}
else if (order == EulerOrder.YZX)
{
var ac = a * c;
var ad = a * d;
var bc = b * c;
var bd = b * d;
result[0] = c * e;
result[4] = bd - ac * f;
result[8] = bc * f + ad;
result[1] = f;
result[5] = a * e;
result[9] = -b * e;
result[2] = -d * e;
result[6] = ad * f + bc;
result[10] = ac - bd * f;
}
else if (order == EulerOrder.XZY)
{
var ac = a * c;
var ad = a * d;
var bc = b * c;
var bd = b * d;
result[0] = c * e;
result[4] = -f;
result[8] = d * e;
result[1] = ac * f + bd;
result[5] = a * e;
result[9] = ad * f - bc;
result[2] = bc * f - ad;
result[6] = b * e;
result[10] = bd * f + ac;
}
// last column
result[3] = 0;
result[7] = 0;
result[11] = 0;
// bottom row
result[12] = 0;
result[13] = 0;
result[14] = 0;
result[15] = 1;
return(result);
}
internal extern static PXCMPoint3DF32 PXCMRotation_QueryEulerAngles(IntPtr instance, EulerOrder order);
