public static void ToEulerAngles(QuaternionR q, out double roll, out double pitch, out double yaw) { double ysqr = q.Y * q.Y; double t0 = 2.0 * (q.W * q.X + q.Y * q.Z); double t1 = 1.0 - 2.0 * (q.X * q.X + ysqr); roll = Math.Atan2(t0, t1); double t2 = 2.0 * (q.W * q.Y - q.Z * q.X); if (t2 > 1.0) { t2 = 1.0; } if (t2 < -1.0) { t2 = -1.0; } pitch = Math.Asin(t2); double t3 = 2.0 * (q.W * q.Z + q.X * q.Y); double t4 = 1.0 - 2.0 * (ysqr + q.Z * q.Z); yaw = Math.Atan2(t3, t4); }
public static Vector3R RotateReverse(Vector3R vector, QuaternionR quat, bool assumeUnitQuat) { Vector3 v = new Vector3(vector.X, vector.Y, vector.Z); Quaternion q = new Quaternion(quat.X, quat.Y, quat.Z, assumeUnitQuat ? 1 : quat.W); Vector3 result = Vector3.Transform(v, Quaternion.Conjugate(q)); return(new Vector3R { X = result.X, Y = result.Y, Z = result.Z }); }
public static QuaternionR Negate(QuaternionR quat) { return(new QuaternionR { X = -quat.X, Y = -quat.Y, Z = -quat.Z, W = -quat.W }); }
public static QuaternionR FlipZAxis(QuaternionR quat) { return(new QuaternionR { X = -quat.X, Y = -quat.Y, Z = quat.Z, W = quat.W }); }
public static Vector3R TransformToWorldFrame(Vector3R vBody, QuaternionR quat, bool assumeUnitQuat = true) { return(Rotate(vBody, quat, assumeUnitQuat)); }
public static Vector3R TransformToBodyFrame(Vector3R vWorld, QuaternionR quat, bool assumeUnitQuat = true) { return(RotateReverse(vWorld, quat, assumeUnitQuat)); }