/// <summary> /// Creates a matrix that represents a spherical linear interpolation from one matrix to another. /// </summary> /// <remarks> /// <para>This is an implementation of interpolation without quaternions.</para> /// <para> /// Given two orthonormal 3x3 matrices this function calculates the shortest possible /// interpolation-path between the two rotations. The interpolation curve forms the shortest /// great arc on the rotation sphere (geodesic). /// </para> /// <para>Angular velocity of the interpolation is constant.</para> /// <para>Possible stability problems:</para> /// <para> /// There are two singularities at angle = 0 and angle = <see cref="Math.PI"/> . At 0 the /// interpolation-axis is arbitrary, which means any axis will produce the same result /// because we have no rotation. (1,0,0) axis is used in this case. At <see cref="Math.PI"/> /// the rotations point away from each other and the interpolation-axis is unpredictable. In /// this case axis (1,0,0) is used as well. If the angle is ~0 or ~PI, then a very small /// vector has to be normalized and this can cause numerical instability. /// </para> /// </remarks> /// <param name="m">First matrix.</param> /// <param name="n">Second matrix.</param> /// <param name="t">Interpolation parameter.</param> public void SetSlerp(Matrix34 m, Matrix34 n, float t) { // calculate delta-rotation between m and n (=39 flops) Matrix33 d = new Matrix33(), i = new Matrix33(); d.M00 = m.M00 * n.M00 + m.M10 * n.M10 + m.M20 * n.M20; d.M01 = m.M00 * n.M01 + m.M10 * n.M11 + m.M20 * n.M21; d.M02 = m.M00 * n.M02 + m.M10 * n.M12 + m.M20 * n.M22; d.M10 = m.M01 * n.M00 + m.M11 * n.M10 + m.M21 * n.M20; d.M11 = m.M01 * n.M01 + m.M11 * n.M11 + m.M21 * n.M21; d.M12 = m.M01 * n.M02 + m.M11 * n.M12 + m.M21 * n.M22; d.M20 = d.M01 * d.M12 - d.M02 * d.M11; d.M21 = d.M02 * d.M10 - d.M00 * d.M12; d.M22 = d.M00 * d.M11 - d.M01 * d.M10; // extract angle and axis double cosine = MathHelpers.Clamp((d.M00 + d.M11 + d.M22 - 1.0) * 0.5, -1.0, +1.0); double angle = Math.Atan2(Math.Sqrt(1.0 - cosine * cosine), cosine); var axis = new Vector3(d.M21 - d.M12, d.M02 - d.M20, d.M10 - d.M01); double l = Math.Sqrt(axis | axis); if (l > 0.00001) { axis /= (float)l; } else { axis = new Vector3(1, 0, 0); } i.SetRotationAroundAxis((float)angle * t, axis); // angle interpolation and calculation of new delta-matrix (=26 flops) // final concatenation (=39 flops) this.M00 = m.M00 * i.M00 + m.M01 * i.M10 + m.M02 * i.M20; this.M01 = m.M00 * i.M01 + m.M01 * i.M11 + m.M02 * i.M21; this.M02 = m.M00 * i.M02 + m.M01 * i.M12 + m.M02 * i.M22; this.M10 = m.M10 * i.M00 + m.M11 * i.M10 + m.M12 * i.M20; this.M11 = m.M10 * i.M01 + m.M11 * i.M11 + m.M12 * i.M21; this.M12 = m.M10 * i.M02 + m.M11 * i.M12 + m.M12 * i.M22; this.M20 = this.M01 * this.M12 - this.M02 * this.M11; this.M21 = this.M02 * this.M10 - this.M00 * this.M12; this.M22 = this.M00 * this.M11 - this.M01 * this.M10; this.M03 = m.M03 * (1 - t) + n.M03 * t; this.M13 = m.M13 * (1 - t) + n.M13 * t; this.M23 = m.M23 * (1 - t) + n.M23 * t; }
/// <summary> /// Sets this vector to be a spherical interpolation defined by two vectors and a value. /// </summary> /// <param name="p"> First vector that defines interpolation. </param> /// <param name="q"> Second vector that defines interpolation. </param> /// <param name="t"> A value between 0 and 1 that defines position of interpolated vector. </param> public void SetSphericalInterpolation(Vector3 p, Vector3 q, float t) { // calculate cosine using the "inner product" between two // vectors: p*q=cos(radiant) float cosine = MathHelpers.Clamp((p | q), -1f, 1f); // Perform normalized linear interpolation if two vectors if they are very close to each // other to avoid division by zero. if (cosine >= 0.99f) { this.SetLinearInterpolation(p, q, t); //perform LERP: this.Normalize(); } else { float rad = (float)Math.Acos(cosine); float scale_0 = (float)Math.Sin((1 - t) * rad); float scale_1 = (float)Math.Sin(t * rad); this = (p * scale_0 + q * scale_1) / (float)Math.Sin(rad); this.Normalize(); } }
/// <summary> /// Creates spherical interpolation defined by two vectors and a value. /// </summary> /// <param name="p">First vector that defines interpolation.</param> /// <param name="q">Second vector that defines interpolation.</param> /// <param name="t">A value between 0 and 1 that defines position of interpolated vector.</param> public static Vector2 CreateSphericalInterpolation(Vector2 p, Vector2 q, float t) { Vector2 result; // calculate cosine using the "inner product" between two // vectors: p*q=cos(radiant) float cosine = MathHelpers.Clamp(Vector2.Dot(p, q), -1f, 1f); // Perform normalized linear interpolation if two vectors if they are very close to each // other to avoid division by zero. if (cosine >= 0.99f) { result = Vector2.CreateLinearInterpolation(p, q, t); //perform LERP: result.Normalize(); } else { float rad = (float)Math.Acos(cosine); float scale0 = (float)Math.Sin((1 - t) * rad); float scale1 = (float)Math.Sin(t * rad); result = (p * scale0 + q * scale1) / (float)Math.Sin(rad); result.Normalize(); } return(result); }