/// <summary> /// Direct-Matrix-Slerp: for the sake of completeness, I have included the following expression /// for Spherical-Linear-Interpolation without using quaternions. This is much faster then converting /// both matrices into quaternions in order to do a quaternion slerp and then converting the slerped /// quaternion back into a matrix. /// This is a high-precision calculation. Given two orthonormal 3x3 matrices this function calculates /// the shortest possible interpolation-path between the two rotations. The interpolation curve forms /// a great arc on the rotation sphere (geodesic). Not only does Slerp follow a great arc it follows /// the shortest great arc. Furthermore Slerp has constant angular velocity. All in all Slerp is the /// optimal interpolation curve between two rotations. /// STABILITY PROBLEM: There are two singularities at angle=0 and angle=PI. At 0 the interpolation-axis /// is arbitrary, which means any axis will produce the same result because we have no rotation. Thats /// why I'm using (1,0,0). At PI the rotations point away from each other and the interpolation-axis /// is unpredictable. In this case I'm also using the axis (1,0,0). If the angle is ~0 or ~PI, then we /// have to normalize a very small vector and this can cause numerical instability. The quaternion-slerp /// has exactly the same problems. Ivo /// </summary> /// <param name="m"></param> /// <param name="n"></param> /// <param name="t"></param> /// <example>Matrix33 slerp=Matrix33::CreateSlerp( m,n,0.333f );</example> 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 Vec3(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 Vec3(1, 0, 0); } i.SetRotationAA((float)angle * t, axis); // angle interpolation and calculation of new delta-matrix (=26 flops) // final concatenation (=39 flops) M00 = m.M00 * i.M00 + m.M01 * i.M10 + m.M02 * i.M20; M01 = m.M00 * i.M01 + m.M01 * i.M11 + m.M02 * i.M21; M02 = m.M00 * i.M02 + m.M01 * i.M12 + m.M02 * i.M22; M10 = m.M10 * i.M00 + m.M11 * i.M10 + m.M12 * i.M20; M11 = m.M10 * i.M01 + m.M11 * i.M11 + m.M12 * i.M21; M12 = m.M10 * i.M02 + m.M11 * i.M12 + m.M12 * i.M22; M20 = M01 * M12 - M02 * M11; M21 = M02 * M10 - M00 * M12; M22 = M00 * M11 - M01 * M10; M03 = m.M03 * (1 - t) + n.M03 * t; M13 = m.M13 * (1 - t) + n.M13 * t; M23 = m.M23 * (1 - t) + n.M23 * t; }
public static Matrix33 CreateRotationAA(Vec3 rot) { var matrix = new Matrix33(); matrix.SetRotationAA(rot); return(matrix); }
public static Matrix33 CreateRotationAA(float c, float s, Vec3 axis) { var matrix = new Matrix33(); matrix.SetRotationAA(c, s, axis); return(matrix); }
/// <summary> /// Direct-Matrix-Slerp: for the sake of completeness, I have included the following expression /// for Spherical-Linear-Interpolation without using quaternions. This is much faster then converting /// both matrices into quaternions in order to do a quaternion slerp and then converting the slerped /// quaternion back into a matrix. /// This is a high-precision calculation. Given two orthonormal 3x3 matrices this function calculates /// the shortest possible interpolation-path between the two rotations. The interpolation curve forms /// a great arc on the rotation sphere (geodesic). Not only does Slerp follow a great arc it follows /// the shortest great arc. Furthermore Slerp has constant angular velocity. All in all Slerp is the /// optimal interpolation curve between two rotations. /// STABILITY PROBLEM: There are two singularities at angle=0 and angle=PI. At 0 the interpolation-axis /// is arbitrary, which means any axis will produce the same result because we have no rotation. Thats /// why I'm using (1,0,0). At PI the rotations point away from each other and the interpolation-axis /// is unpredictable. In this case I'm also using the axis (1,0,0). If the angle is ~0 or ~PI, then we /// have to normalize a very small vector and this can cause numerical instability. The quaternion-slerp /// has exactly the same problems. Ivo /// </summary> /// <param name="m"></param> /// <param name="n"></param> /// <param name="t"></param> /// <example>Matrix33 slerp=Matrix33::CreateSlerp( m,n,0.333f );</example> 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 Vec3(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 Vec3(1, 0, 0); i.SetRotationAA((float)angle * t, axis); // angle interpolation and calculation of new delta-matrix (=26 flops) // final concatenation (=39 flops) M00 = m.M00 * i.M00 + m.M01 * i.M10 + m.M02 * i.M20; M01 = m.M00 * i.M01 + m.M01 * i.M11 + m.M02 * i.M21; M02 = m.M00 * i.M02 + m.M01 * i.M12 + m.M02 * i.M22; M10 = m.M10 * i.M00 + m.M11 * i.M10 + m.M12 * i.M20; M11 = m.M10 * i.M01 + m.M11 * i.M11 + m.M12 * i.M21; M12 = m.M10 * i.M02 + m.M11 * i.M12 + m.M12 * i.M22; M20 = M01 * M12 - M02 * M11; M21 = M02 * M10 - M00 * M12; M22 = M00 * M11 - M01 * M10; M03 = m.M03 * (1 - t) + n.M03 * t; M13 = m.M13 * (1 - t) + n.M13 * t; M23 = m.M23 * (1 - t) + n.M23 * t; }
public static Matrix33 CreateRotationAA(Vec3 rot) { var matrix = new Matrix33(); matrix.SetRotationAA(rot); return matrix; }
public static Matrix33 CreateRotationAA(float c, float s, Vec3 axis) { var matrix = new Matrix33(); matrix.SetRotationAA(c, s, axis); return matrix; }