/// <summary>
/// Adds this <see cref="vec4f"/> with the provided
/// <see cref="vec4f"/> together.
/// </summary>
/// <param name="vector">
/// The <see cref="vec4f"/> to add to this <see cref="vec4f"/>.
/// </param>
/// <returns>
/// The resultant <see cref="vec4f"/> from the addition.
/// </returns>
public vec4f Add(vec4f vector)
{
return(new vec4f(
X + vector.X,
Y + vector.Y,
Z + vector.Z,
W + vector.W));
}
/// <summary>
/// Subtracts the provided <see cref="vec4f"/> from this
/// <see cref="vec4f"/>.
/// </summary>
/// <param name="rhs">
/// The <see cref="vec4f"/> to subtract from this
/// <see cref="vec4f"/>.
/// </param>
/// <returns>
/// The resultant <see cref="vec4f"/> from the subtraction.
/// </returns>
public vec4f Subtract(vec4f rhs)
{
return(new vec4f(
X - rhs.X,
Y - rhs.Y,
Z - rhs.Z,
W - rhs.W));
}
/// <summary>
/// Converts an orientation represented as a quaternion to yaw, pitch,
/// roll values in radians.
/// </summary>
/// <param name="quat">
/// The quaternion value to convert.
/// </param>
/// <returns>
/// The corresponding yaw, pitch, roll values in radians.
/// </returns>
public static vec3f Quat2YprInRads(vec4f quat)
{
var q1 = quat.X;
var q2 = quat.Y;
var q3 = quat.Z;
var q0 = quat.W;
return(new vec3f(
(float)System.Math.Atan2(2.0f * (q1 * q2 + q0 * q3), q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3),
(float)System.Math.Asin(-2.0 * (q1 * q3 - q0 * q2)),
(float)System.Math.Atan2(2.0 * (q2 * q3 + q0 * q1), q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3)));
}
/// <summary>
/// Converts an orientation represented as a direction cosine matrix to a
/// quaternion.
/// </summary>
/// <param name="dcm">
/// The direction cosine matrix to convert.
/// </param>
/// <returns>
/// The corresponding quaternion.
/// </returns>
public static vec4f Dcm2Quat(mat3f dcm)
{
var tr = dcm.E00 + dcm.E11 + dcm.E22;
var b2 = new[] {
(1f + tr) / 4f,
(1f + 2f * dcm.E00 - tr) / 4f,
(1f + 2f * dcm.E11 - tr) / 4f,
(1f + 2f * dcm.E22 - tr) / 4f
};
var maxNum = b2[0];
var maxIndex = 0;
for (var i = 1; i < b2.Length; i++)
{
if (b2[i] > maxNum)
{
maxNum = b2[i];
maxIndex = i;
}
}
var q = new vec4f();
switch (maxIndex)
{
case 0:
q.W = (float)System.Math.Sqrt(b2[0]);
q.X = (dcm.E12 - dcm.E21) / 4f / q.W;
q.Y = (dcm.E20 - dcm.E02) / 4f / q.W;
q.Z = (dcm.E01 - dcm.E10) / 4f / q.W;
break;
case 1:
q.X = (float)System.Math.Sqrt(b2[1]);
q.W = (dcm.E12 - dcm.E21) / 4f / q.X;
if (q.W < 0)
{
q.X = -q.X;
q.W = -q.W;
}
q.Y = (dcm.E01 + dcm.E10) / 4f / q.X;
q.Z = (dcm.E20 + dcm.E02) / 4f / q.X;
break;
case 2:
q.Y = (float)System.Math.Sqrt(b2[2]);
q.W = (dcm.E20 - dcm.E02) / 4f / q.Y;
if (q.W < 0)
{
q.Y = -q.Y;
q.W = -q.W;
}
q.X = (dcm.E01 + dcm.E10) / 4f / q.Y;
q.Z = (dcm.E12 + dcm.E21) / 4f / q.Y;
break;
case 3:
q.Z = (float)System.Math.Sqrt(b2[3]);
q.W = (dcm.E01 - dcm.E10) / 4f / q.Z;
if (q.W < 0)
{
q.Z = -q.Z;
q.W = -q.W;
}
q.X = (dcm.E20 + dcm.E02) / 4f / q.Z;
q.Y = (dcm.E12 + dcm.E21) / 4f / q.Z;
break;
}
return(q);
}
/// <summary>
/// Converts an orientation represented as a quaternion to yaw, pitch,
/// roll values in degrees.
/// </summary>
/// <param name="quat">
/// The quaternion value to convert.
/// </param>
/// <returns>
/// The corresponding yaw, pitch, roll values in degrees.
/// </returns>
public static vec3f Quat2YprInDegs(vec4f quat)
{
return(Rad2Deg(Quat2YprInRads(quat)));
}
/// <summary>
/// Computes the dot product of this <see cref="vec4f"/> and the
/// provided <see cref="vec4f"/>.
/// </summary>
/// <param name="vector">
/// The <see cref="vec4f"/> to compute the dot product with this.
/// </param>
/// <returns>
/// The computed dot product.
/// </returns>
public float DotProduct(vec4f vector)
{
return(X * vector.X + Y * vector.Y + Z * vector.Z + W * vector.W);
}
