/// <summary>
/// Creates new quaternion that represents rotation defined by given 3x3 matrix.
/// </summary>
/// <param name="m">Matrix that defines the rotation.</param>
/// <returns>
/// Quaternion that represents rotation defined by given 3x3 matrix.
/// </returns>
public static Quaternion FromMatrix33(Matrix33 m)
{
float s, p, tr = m.M00 + m.M11 + m.M22;
//check the diagonal
if (tr > (float)0.0)
{
s = (float)Math.Sqrt(tr + 1.0f); p = 0.5f / s;
return(new Quaternion(s * 0.5f, (m.M21 - m.M12) * p, (m.M02 - m.M20) * p, (m.M10 - m.M01) * p));
}
//diagonal is negative. now we have to find the biggest element on the diagonal
//check if "M00" is the biggest element
if ((m.M00 >= m.M11) && (m.M00 >= m.M22))
{
s = (float)Math.Sqrt(m.M00 - m.M11 - m.M22 + 1.0f); p = 0.5f / s;
return(new Quaternion((m.M21 - m.M12) * p, s * 0.5f, (m.M10 + m.M01) * p, (m.M20 + m.M02) * p));
}
//check if "M11" is the biggest element
if ((m.M11 >= m.M00) && (m.M11 >= m.M22))
{
s = (float)Math.Sqrt(m.M11 - m.M22 - m.M00 + 1.0f); p = 0.5f / s;
return(new Quaternion((m.M02 - m.M20) * p, (m.M01 + m.M10) * p, s * 0.5f, (m.M21 + m.M12) * p));
}
//check if "M22" is the biggest element
if ((m.M22 >= m.M00) && (m.M22 >= m.M11))
{
s = (float)Math.Sqrt(m.M22 - m.M00 - m.M11 + 1.0f); p = 0.5f / s;
return(new Quaternion((m.M10 - m.M01) * p, (m.M02 + m.M20) * p, (m.M12 + m.M21) * p, s * 0.5f));
}
return(Quaternion.Identity); // if it ends here, then we have no valid rotation matrix
}
/// <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>
/// Creates new instance of type <see cref="Matrix34"/> .
/// </summary>
/// <param name="m33">
/// <see cref="CryCil.Mathematics.Matrix33"/> to use to fill first 3 columns.
/// </param>
public Matrix34(Matrix33 m33)
: this()
{
this.Row0 = new Vector4(m33.Row0, 0);
this.Row1 = new Vector4(m33.Row1, 0);
this.Row2 = new Vector4(m33.Row2, 1);
}
/// <summary>
/// Creates new matrix that is set to represent scaling operation.
/// </summary>
/// <param name="s">Scale to set.</param>
/// <returns>New matrix.</returns>
public static Matrix33 CreateScale(Vector3 s)
{
var matrix = new Matrix33();
matrix.SetScale(s);
return(matrix);
}
/// <summary>
/// Creates new matrix that is set to represent rotation around fixed Axes.
/// </summary>
/// <param name="rad">Angles of rotation.</param>
/// <returns>New matrix.</returns>
public static Matrix33 CreateRotationFromAngles(EulerAngles rad)
{
var matrix = new Matrix33();
matrix.SetRotationFromAngles(rad);
return(matrix);
}
/// <summary>
/// Creates new matrix that is set to represent rotation around Z axis.
/// </summary>
/// <param name="rad">Angle of rotation around Z axis.</param>
/// <returns>New matrix.</returns>
public static Matrix33 CreateRotationAroundZ(float rad)
{
var matrix = new Matrix33();
matrix.SetRotationAroundZ(rad);
return(matrix);
}
/// <summary>
/// Creates new matrix that is set to represent rotation around given axis.
/// </summary>
/// <param name="rot">
/// <see cref="Vector3"/> which length represents an angle of rotation, and that,
/// once normalized, represents an axis of rotation.
/// </param>
/// <returns>New matrix.</returns>
public static Matrix33 CreateRotationAroundAxis(Vector3 rot)
{
var matrix = new Matrix33();
matrix.SetRotationAroundAxis(rot);
return(matrix);
}
/// <summary>
/// Creates new matrix that is set to represent rotation around given axis.
/// </summary>
/// <param name="c"> Cosine of angle of rotation.</param>
/// <param name="s"> Sine of angle of rotation.</param>
/// <param name="axis">Axis of rotation.</param>
/// <returns>New matrix.</returns>
public static Matrix33 CreateRotationAroundAxis(float c, float s, Vector3 axis)
{
var matrix = new Matrix33();
matrix.SetRotationAroundAxis(c, s, axis);
return(matrix);
}
/// <summary>
/// Creates new matrix from vectors that represent rows.
/// </summary>
/// <param name="vx"><see cref="Vector3"/> object that contains first row.</param>
/// <param name="vy">
/// <see cref="Vector3"/> object that contains second row.
/// </param>
/// <param name="vz"><see cref="Vector3"/> object that contains third row.</param>
/// <returns>New matrix.</returns>
public static Matrix33 CreateFromVectors(Vector3 vx, Vector3 vy, Vector3 vz)
{
var matrix = new Matrix33();
matrix.SetFromVectors(vx, vy, vz);
return(matrix);
}
/// <summary>
/// Sets the values of this matrix to one that represents a rotation around an
/// axis.
/// </summary>
/// <param name="c"> Cosine of the angle of the rotation.</param>
/// <param name="s"> Sine of the angle of the rotation.</param>
/// <param name="axis">
/// <see cref="Vector3"/> object that represents axis of rotation.
/// </param>
/// <param name="t"> Optional translation vector.</param>
public void SetRotationAroundAxis(float c, float s, Vector3 axis, Vector3 t = default(Vector3))
{
this = new Matrix34(Matrix33.CreateRotationAroundAxis(c, s, axis))
{
M03 = t.X,
M13 = t.Y,
M23 = t.Z
};
}
/// <summary>
/// Creates new instance of <see cref="Matrix44" /> class.
/// </summary>
/// <param name="m"> <see cref="Matrix33" /> to fill new matrix with. </param>
public Matrix44(ref Matrix33 m)
: this()
{
if (!m.IsValid)
{
throw new ArgumentException("Parameter must be a valid matrix.");
}
this.Row0.X = m.Row1.X; this.Row0.Y = m.Row1.Y; this.Row0.Z = m.Row1.Z; this.Row0.W = 0;
this.Row1.X = m.Row2.X; this.Row1.Y = m.Row2.Y; this.Row1.Z = m.Row2.Z; this.Row1.W = 0;
this.Row2.X = m.Row2.X; this.Row2.Y = m.Row2.Y; this.Row2.Z = m.Row2.Z; this.Row2.W = 0;
this.Row3.X = 0; this.Row3.Y = 0; this.Row3.Z = 0; this.Row3.W = 1;
}
/// <summary>
/// Creates new instance of <see cref="EulerAngles"/> struct.
/// </summary>
/// <param name="matrix">Matrix that defines new instance.</param>
public EulerAngles(Matrix33 matrix)
{
// Assert matrix being orthonormal.
this.Roll = (float)Math.Asin(Math.Max(-1.0f, Math.Min(1.0f, -matrix.M20)));
if (Math.Abs(Math.Abs(this.Roll) - (float)(Math.PI * 0.5)) < 0.01f)
{
this.Pitch = 0;
this.Yaw = (float)Math.Atan2(-matrix.M01, matrix.M11);
}
else
{
this.Pitch = (float)Math.Atan2(matrix.M21, matrix.M22);
this.Yaw = (float)Math.Atan2(matrix.M10, matrix.M00);
}
}
/// <summary>
/// Multiplies the matrix by other matrix.
/// </summary>
/// <param name="left"> Left operand.</param>
/// <param name="right">Right opearnd.</param>
/// <returns>Result of operation.</returns>
public static Matrix33 operator *(Matrix33 left, Matrix33 right)
{
var m = new Matrix33
{
M00 = left.M00 * right.M00 + left.M01 * right.M10 + left.M02 * right.M20,
M01 = left.M00 * right.M01 + left.M01 * right.M11 + left.M02 * right.M21,
M02 = left.M00 * right.M02 + left.M01 * right.M12 + left.M02 * right.M22,
M10 = left.M10 * right.M00 + left.M11 * right.M10 + left.M12 * right.M20,
M11 = left.M10 * right.M01 + left.M11 * right.M11 + left.M12 * right.M21,
M12 = left.M10 * right.M02 + left.M11 * right.M12 + left.M12 * right.M22,
M20 = left.M20 * right.M00 + left.M21 * right.M10 + left.M22 * right.M20,
M21 = left.M20 * right.M01 + left.M21 * right.M11 + left.M22 * right.M21,
M22 = left.M20 * right.M02 + left.M21 * right.M12 + left.M22 * right.M22
};
return(m);
}
/// <summary>
/// Creates new instance of <see cref="Quaternion"/> struct.
/// </summary>
/// <param name="matrix">3x3 matrix that represents rotation.</param>
public Quaternion(Matrix33 matrix)
{
this = FromMatrix33(matrix);
}
/// <summary>
/// Convert three Euler angles to 3x3 matrix (rotation order:XYZ)
/// </summary>
/// <param name="rad">Angles of rotation.</param>
/// <param name="t"> Optional translation vector.</param>
public void SetRotationWithEulerAngles(EulerAngles rad, Vector3 t = default(Vector3))
{
this = new Matrix34(Matrix33.CreateRotationFromAngles(rad));
this.SetTranslation(t);
}
/// <summary>
/// Sets the values of this matrix to one that represents a rotation around Z
/// axis.
/// </summary>
/// <param name="rad">Angle of rotation in radians.</param>
/// <param name="t"> Optional translation vector.</param>
public void SetRotationAroundZ(float rad, Vector3 t = default(Vector3))
{
this = new Matrix34(Matrix33.CreateRotationAroundZ(rad));
this.SetTranslation(t);
}
/// <summary>
/// Sets the values of this matrix to one that represents a rotation around an
/// axis.
/// </summary>
/// <param name="rot">
/// <see cref="Vector3"/> object which length represents an angle of rotation and
/// which direction represents an axis of rotation.
/// </param>
/// <param name="t"> Optional translation vector.</param>
public void SetRotationAroundAxis(Vector3 rot, Vector3 t = default(Vector3))
{
this = new Matrix34(Matrix33.CreateRotationAroundAxis(rot));
this.SetTranslation(t);
}
/// <summary>
/// Sets the value of the matrix to one that represents scaling.
/// </summary>
/// <param name="s"><see cref="Vector3"/> object that represents scale.</param>
/// <param name="t">
/// Optional <see cref="Vector3"/> object that represents translation.
/// </param>
public void SetScale(Vector3 s, Vector3 t = default(Vector3))
{
this = new Matrix34(Matrix33.CreateScale(s));
this.SetTranslation(t);
}
