/// <summary> /// Rotate this vector about the given axis angle. /// </summary> /// <param name="wAxis">The axis to rotate about. Must be unit length.</param> /// <param name="angle">The angle of rotation.</param> /// <returns></returns> public Vector3 rotate(Vector3 wAxis, float angle) { // wAxis must be a unit lenght vector Vector3 o = wAxis * wAxis.dot(ref this); Vector3 x = this - o; Vector3 y; y = wAxis.cross(ref this); return(o + x * (float)System.Math.Cos(angle) + y * (float)System.Math.Sin(angle)); }
/// <summary> /// Find the shortest arc between two vectors. /// </summary> /// <param name="v0">Source vector.</param> /// <param name="v1">Destination vector.</param> /// <returns>The quaternion with the shortest arc.</returns> public static Quaternion shortestArcQuat(ref Vector3 v0, ref Vector3 v1) { Vector3 c = v0.cross(ref v1); float d = v0.dot(ref v1); if (d < -1.0 + FLT_EPSILON) { return(new Quaternion(0.0f, 1.0f, 0.0f, 0.0f)); // just pick any vector } float s = (float)System.Math.Sqrt((1.0f + d) * 2.0f); float rs = 1.0f / s; return(new Quaternion(c.x * rs, c.y * rs, c.z * rs, s * 0.5f).normalized()); }
/// <summary> /// Compute the shortest arc quaternion with a fixed yaw axis. /// </summary> /// <param name="direction">The direction to face. Must be normalized.</param> /// <param name="yawFixedAxis">The axis to fix as yaw.</param> /// <returns>The quaternion that will get to this orientation.</returns> public static Quaternion shortestArcQuatFixedYaw(ref Vector3 direction, ref Vector3 yawFixedAxis) { Vector3 xVec = yawFixedAxis.cross(ref direction); xVec.normalize(); Vector3 yVec = direction.cross(ref xVec); yVec.normalize(); Quaternion targetWorldOrientation = new Quaternion(); targetWorldOrientation.fromAxes(xVec, yVec, direction); return(targetWorldOrientation); }