/// <summary>
/// Combines the euler angles in the order roll, pitch, yaw to create a rotation quaternion.
/// This means that if you wrote this in standard math form, it would be Qy * Qp * Qr.
/// </summary>
/// <param name="pitch"></param>
/// <param name="yaw"></param>
/// <param name="roll"></param>
/// <returns></returns>
public static Quaternion FromEulerAngles(float pitch, float yaw, float roll)
{
return(Quaternion.FromAngleAxis(yaw, Vector3.UnitY)
* Quaternion.FromAngleAxis(pitch, Vector3.UnitX)
* Quaternion.FromAngleAxis(roll, Vector3.UnitZ));
/*TODO: Debug
* //Equation from http://www.euclideanspace.com/maths/geometry/rotations/conversions/eulerToQuaternion/index.htm
* //heading
*
* float c1 = (float)Math.Cos(yaw/2);
* float s1 = (float)Math.Sin(yaw/2);
* //attitude
* float c2 = (float)Math.Cos(roll/2);
* float s2 = (float)Math.Sin(roll/2);
* //bank
* float c3 = (float)Math.Cos(pitch/2);
* float s3 = (float)Math.Sin(pitch/2);
* float c1c2 = c1*c2;
* float s1s2 = s1*s2;
*
* float w =c1c2*c3 - s1s2*s3;
* float x =c1c2*s3 + s1s2*c3;
* float y =s1*c2*c3 + c1*s2*s3;
* float z =c1*s2*c3 - s1*c2*s3;
* return new Quaternion(w,x,y,z);*/
}
/// <summary>
/// Gets the shortest arc quaternion to rotate this vector
/// to the destination vector.
/// </summary>
/// <remarks>
/// If you call this with a dest vector that is close to the inverse
/// of this vector, we will rotate 180 degrees around the 'fallbackAxis'
/// (if specified, or a generated axis if not) since in this case
/// ANY axis of rotation is valid.
/// </remarks>
public Quaternion GetRotationTo(Vector3 destination, Vector3 fallbackAxis)
{
// Based on Stan Melax's article in Game Programming Gems
Quaternion q = new Quaternion();
Vector3 v0 = new Vector3(this.x, this.y, this.z);
Vector3 v1 = destination;
// normalize both vectors
v0.Normalize();
v1.Normalize();
// get the cross product of the vectors
Vector3 c = v0.Cross(v1);
// If the cross product approaches zero, we get unstable because ANY axis will do
// when v0 == -v1
float d = v0.Dot(v1);
// If dot == 1, vectors are the same
if (d >= 1.0f)
{
return(Quaternion.Identity);
}
if (d < (1e-6f - 1.0f))
{
if (fallbackAxis != Vector3.Zero)
{
// rotate 180 degrees about the fallback axis
q = Quaternion.FromAngleAxis((float)Math.PI, fallbackAxis);
}
else
{
// Generate an axis
Vector3 axis = Vector3.UnitX.Cross(this);
if (axis.IsZero) // pick another if colinear
{
axis = Vector3.UnitY.Cross(this);
}
axis.Normalize();
q = Quaternion.FromAngleAxis((float)Math.PI, axis);
}
}
else
{
float s = MathUtil.Sqrt((1 + d) * 2);
float inverse = 1 / s;
q.x = c.x * inverse;
q.y = c.y * inverse;
q.z = c.z * inverse;
q.w = s * 0.5f;
q.Normalize();
}
return(q);
}
/// <summary>
///
/// </summary>
/// <param name="angle"></param>
/// <param name="up"></param>
/// <returns></returns>
public Vector3 RandomDeviant(float angle, Vector3 up)
{
Vector3 newUp = Vector3.Zero;
if (up == Vector3.Zero)
{
newUp = this.Perpendicular();
}
else
{
newUp = up;
}
// rotate up vector by random amount around this
Quaternion q = Quaternion.FromAngleAxis(MathUtil.UnitRandom() * MathUtil.TWO_PI, this);
newUp = q * newUp;
// finally, rotate this by given angle around randomized up vector
q = Quaternion.FromAngleAxis(angle, newUp);
return(q * this);
}
