/// <summary> /// Do Spherical linear interpolation between two quaternions /// </summary> /// <param name="q1">The first quaternion</param> /// <param name="q2">The second quaternion</param> /// <param name="blend">The blend factor</param> /// <returns>A smooth blend between the given quaternions</returns> public static Quaternion4F OtkSlerp(Quaternion4F q1, Quaternion4F q2, float blend) { // if either input is zero, return the other. if (q1.LengthSquared == 0.0f) { if (q2.LengthSquared == 0.0f) { return(Identity); } return(q2); } if (q2.LengthSquared == 0.0f) { return(q1); } var cosHalfAngle = q1.W * q2.W + q1.Xyz.Dot(q2.Xyz); if (cosHalfAngle >= 1.0f || cosHalfAngle <= -1.0f) { // angle = 0.0f, so just return one input. return(q1); } if (cosHalfAngle < 0.0f) { q2.Xyz.Negate(); q2.W = -q2.W; cosHalfAngle = -cosHalfAngle; } float blendA; float blendB; if (cosHalfAngle < 0.99f) { // do proper slerp for big angles var halfAngle = (float)Math.Acos(cosHalfAngle); var sinHalfAngle = (float)Math.Sin(halfAngle); var oneOverSinHalfAngle = 1.0f / sinHalfAngle; blendA = (float)Math.Sin(halfAngle * (1.0f - blend)) * oneOverSinHalfAngle; blendB = (float)Math.Sin(halfAngle * blend) * oneOverSinHalfAngle; } else { // do lerp if angle is really small. blendA = 1.0f - blend; blendB = blend; } var result = new Quaternion4F(blendA * q1.Xyz + blendB * q2.Xyz, blendA * q1.W + blendB * q2.W); if (result.LengthSquared > 0.0f) { result.Normalize(); return(result); } return(Identity); }
public static Quaternion4F FromRadiansAxis(float radianes, float x, float y, float z) { var quaternion = new Quaternion4F(); var num2 = radianes * 0.5f; var num = (float)Math.Sin(num2); var num3 = (float)Math.Cos(num2); quaternion._x = x * num; quaternion._y = y * num; quaternion._z = z * num; quaternion._w = num3; quaternion.Normalize(); return(quaternion); }