/// <summary>
/// Produces a set of <see cref="UnitCartesian"/> coordinates representing this instance which results from rotating
/// the original axes used to represent this instance by the provided <see cref="Matrix3By3"/> rotation.
/// </summary>
/// <param name="rotation">The <see cref="Matrix3By3"/> rotation.</param>
/// <returns>A set of <see cref="UnitCartesian"/> coordinates which is the result of the rotation.</returns>
/// <remarks>
/// This type of rotation is sometimes referred to as an "alias rotation".
/// </remarks>
public UnitCartesian Rotate(Matrix3By3 rotation)
{
return(new UnitCartesian(rotation.M11 * m_x + rotation.M12 * m_y + rotation.M13 * m_z,
rotation.M21 * m_x + rotation.M22 * m_y + rotation.M23 * m_z,
rotation.M31 * m_x + rotation.M32 * m_y + rotation.M33 * m_z,
Normalization.Normalized));
}
public Cartesian Rotate(Matrix3By3 rotation)
{
return(new Cartesian(rotation.M11 * m_x + rotation.M12 * m_y + rotation.M13 * m_z,
rotation.M21 * m_x + rotation.M22 * m_y + rotation.M23 * m_z,
rotation.M31 * m_x + rotation.M32 * m_y + rotation.M33 * m_z));
}
/// <summary>
/// Initializes a set of <see cref="UnitQuaternion"/> coordinates from the provided rotation matrix (<see cref="Matrix3By3"/>).
/// Note that if the given <paramref name="matrix"/> is not an orthogonal rotation matrix,
/// it will create a non-unit <see cref="UnitQuaternion"/> and could cause problems in code which assumes that the <see cref="UnitQuaternion"/> represents a rotation.
/// </summary>
/// <param name="matrix">The 3-by-3 rotation matrix.</param>
/// <remarks>For performance reasons, there is no check to ensure that the <paramref name="matrix"/> is a unit rotation prior
/// to converting to a unit quaternion. If necessary, the surrounding code is responsible for ensuring that the given
/// <paramref name="matrix"/> is a valid orthogonal rotation matrix.</remarks>
public UnitQuaternion(Matrix3By3 matrix)
{
double factor = matrix.M11 + matrix.M22 + matrix.M33;
int type = 0;
if (matrix.M11 > factor)
{
type = 1;
factor = matrix.M11;
}
if (matrix.M22 > factor)
{
type = 2;
factor = matrix.M22;
}
if (matrix.M33 > factor)
{
type = 3;
factor = matrix.M33;
}
if (type == 1)
{
m_x = 0.5 * Math.Sqrt(1.0 + matrix.M11 - matrix.M22 - matrix.M33);
factor = 1.0 / (4.0 * m_x);
m_w = factor * (matrix.M23 - matrix.M32);
if (m_w < 0)
{
m_w = -m_w;
m_x = -m_x;
factor = -factor;
}
m_y = factor * (matrix.M12 + matrix.M21);
m_z = factor * (matrix.M13 + matrix.M31);
}
else if (type == 2)
{
m_y = 0.5 * Math.Sqrt(1.0 - matrix.M11 + matrix.M22 - matrix.M33);
factor = 1.0 / (4.0 * m_y);
m_w = factor * (matrix.M31 - matrix.M13);
if (m_w < 0)
{
m_w = -m_w;
m_y = -m_y;
factor = -factor;
}
m_x = factor * (matrix.M12 + matrix.M21);
m_z = factor * (matrix.M23 + matrix.M32);
}
else if (type == 3)
{
m_z = 0.5 * Math.Sqrt(1.0 - matrix.M11 - matrix.M22 + matrix.M33);
factor = 1.0 / (4.0 * m_z);
m_w = factor * (matrix.M12 - matrix.M21);
if (m_w < 0)
{
m_w = -m_w;
m_z = -m_z;
factor = -factor;
}
m_x = factor * (matrix.M13 + matrix.M31);
m_y = factor * (matrix.M23 + matrix.M32);
}
else
{
m_w = 0.5 * Math.Sqrt(1.0 + factor);
factor = 1.0 / (4.0 * m_w);
m_x = factor * (matrix.M23 - matrix.M32);
m_y = factor * (matrix.M31 - matrix.M13);
m_z = factor * (matrix.M12 - matrix.M21);
}
}
