|
public static FromAngleAxis ( Real angle, Vector3 axis ) : |
||
| angle | Real | Value of an angle in radians. |
| axis | Vector3 | Arbitrary axis vector. |
| return | ||
/// <summary>
/// Combines the euler angles in the order yaw, pitch, roll to create a rotation quaternion
/// </summary>
/// <param name="pitch"></param>
/// <param name="yaw"></param>
/// <param name="roll"></param>
/// <returns></returns>
public static Quaternion FromEulerAngles(Real pitch, Real yaw, Real 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
*
* Real c1 = (Real)Math.Cos(yaw/2);
* Real s1 = (Real)Math.Sin(yaw/2);
* //attitude
* Real c2 = (Real)Math.Cos(roll/2);
* Real s2 = (Real)Math.Sin(roll/2);
* //bank
* Real c3 = (Real)Math.Cos(pitch/2);
* Real s3 = (Real)Math.Sin(pitch/2);
* Real c1c2 = c1*c2;
* Real s1s2 = s1*s2;
*
* Real w =c1c2*c3 - s1s2*s3;
* Real x =c1c2*s3 + s1s2*c3;
* Real y =s1*c2*c3 + c1*s2*s3;
* Real 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>
/// <param name="destination"></param>
/// <param name="fallbackAxis"></param>
/// <returns></returns>
/// <remarks>
/// Don't call this if you think the dest vector can be close to the inverse
/// of this vector, since then ANY axis of rotation is ok.
/// </remarks>
public Quaternion GetRotationTo(Vector3 destination, Vector3 fallbackAxis)
{
// Based on Stan Melax's article in Game Programming Gems
Quaternion q;
// Copy, since cannot modify local
var v0 = new Vector3(this.x, this.y, this.z);
var v1 = destination;
// normalize both vectors
v0.Normalize();
v1.Normalize();
// get the cross product of the vectors
var c = v0.Cross(v1);
var 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(Utility.PI, fallbackAxis);
}
else
{
// Generate an axis
var axis = Vector3.UnitX.Cross(this);
if (axis.IsZeroLength) // pick another if colinear
{
axis = Vector3.UnitY.Cross(this);
}
axis.Normalize();
q = Quaternion.FromAngleAxis(Utility.PI, axis);
}
}
else
{
var s = Utility.Sqrt((1 + d) * 2);
var inverse = 1 / s;
q.x = c.x * inverse;
q.y = c.y * inverse;
q.z = c.z * inverse;
q.w = s * 0.5f;
}
return(q);
}
/// <summary>
///
/// </summary>
/// <param name="angle"></param>
/// <param name="up"></param>
/// <returns></returns>
public Vector3 RandomDeviant(Real angle, Vector3 up)
{
var newUp = (up == Vector3.Zero) ? Perpendicular() : up;
// rotate up vector by random amount around this
var q = Quaternion.FromAngleAxis(Utility.UnitRandom() * Utility.TWO_PI, this);
newUp = q * newUp;
// finally, rotate this by given angle around randomized up vector
q = Quaternion.FromAngleAxis(angle, newUp);
return(q * this);
}