/// <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); }