/// <summary>
/// Adds this <see cref="vec3f"/> with the provided
/// <see cref="vec3f"/> together.
/// </summary>
/// <param name="vector">
/// The <see cref="vec3f"/> to add to this <see cref="vec3f"/>.
/// </param>
/// <returns>
/// The resultant <see cref="vec3f"/> from the addition.
/// </returns>
public vec3f Add(vec3f vector)
{
return(new vec3f(
X + vector.X,
Y + vector.Y,
Z + vector.Z));
}
/// <summary>
/// Computes the cross product of this <see cref="vec3f"/> and the
/// provided <see cref="vec3f"/>.
/// </summary>
/// <param name="rhs">
/// The <see cref="vec3f"/> to compute the cross product with this.
/// </param>
/// <returns>
/// The computed cross product.
/// </returns>
public vec3f CrossProduct(vec3f rhs)
{
return(new vec3f(
Y * rhs.Z - Z * rhs.Y,
Z * rhs.X - X * rhs.Z,
X * rhs.Y - Y * rhs.X));
}
/// <summary>
/// Subtracts the provided <see cref="vec3f"/> from this
/// <see cref="vec3f"/>.
/// </summary>
/// <param name="rhs">
/// The <see cref="vec3f"/> to subtract from this
/// <see cref="vec3f"/>.
/// </param>
/// <returns>
/// The resultant <see cref="vec3f"/> from the subtraction.
/// </returns>
public vec3f Subtract(vec3f rhs)
{
return(new vec3f(
X - rhs.X,
Y - rhs.Y,
Z - rhs.Z));
}
/// <summary>
/// Converts the collection of angles in radians to degrees.
/// </summary>
/// <param name="anglesInRads">
/// The angles in radians.
/// </param>
/// <returns>
/// The corresponding angles in degrees.
/// </returns>
public static vec3f Rad2Deg(vec3f anglesInRads)
{
return(new vec3f(
Rad2Deg(anglesInRads.X),
Rad2Deg(anglesInRads.Y),
Rad2Deg(anglesInRads.Z)));
}
/// <summary>
/// Converts the collection of angles in degrees to radians.
/// </summary>
/// <param name="anglesInDegs">
/// The angles in degrees.
/// </param>
/// <returns>
/// The corresponding angles in radians.
/// </returns>
public static vec3f Deg2Rad(vec3f anglesInDegs)
{
return(new vec3f(
Deg2Rad(anglesInDegs.X),
Deg2Rad(anglesInDegs.Y),
Deg2Rad(anglesInDegs.Z)));
}
/// <summary>
/// Converts an orientation represented as a yaw, pitch, roll in radians to
/// a quaternion.
/// </summary>
/// <param name="ypr">
/// The yaw, pitch, roll values in radians to convert.
/// </param>
/// <returns>
/// The corresponding quaternion.
/// </returns>
public static vec4f YprInRads2Quat(vec3f ypr)
{
var c1 = (float)System.Math.Cos(ypr.X / 2.0f);
var s1 = (float)System.Math.Sin(ypr.X / 2.0f);
var c2 = (float)System.Math.Cos(ypr.Y / 2.0f);
var s2 = (float)System.Math.Sin(ypr.Y / 2.0f);
var c3 = (float)System.Math.Cos(ypr.Z / 2.0f);
var s3 = (float)System.Math.Sin(ypr.Z / 2.0f);
return(new vec4f(
c1 * c2 * s3 - s1 * s2 * c3,
c1 * s2 * c3 + s1 * c2 * s3,
s1 * c2 * c3 - c1 * s2 * s3,
c1 * c2 * c3 + s1 * s2 * s3));
}
/// <summary>
/// Converts a yaw, pitch, roll in radians to a direction cosine matrix.
/// </summary>
/// <param name="yprInRads">
/// The yaw, pitch, roll rotation expressed in radians.
/// </param>
/// <returns>
/// The corresponding direction cosine matrix.
/// </returns>
public static mat3f YprInRads2Dcm(vec3f yprInRads)
{
var st1 = (float)SMath.Sin(yprInRads.X);
var ct1 = (float)SMath.Cos(yprInRads.X);
var st2 = (float)SMath.Sin(yprInRads.Y);
var ct2 = (float)SMath.Cos(yprInRads.Y);
var st3 = (float)SMath.Sin(yprInRads.Z);
var ct3 = (float)SMath.Cos(yprInRads.Z);
return(new mat3f(
ct2 * ct1,
ct2 * st1,
-st2,
st3 * st2 * ct1 - ct3 * st1,
st3 * st2 * st1 + ct3 * ct1,
st3 * ct2,
ct3 * st2 * ct1 + st3 * st1,
ct3 * st2 * st1 - st3 * ct1,
ct3 * ct2));
}
/// <summary>
/// Computes the dot product of this <see cref="vec3f"/> and the
/// provided <see cref="vec3f"/>.
/// </summary>
/// <param name="vector">
/// The <see cref="vec3f"/> to compute the dot product with this.
/// </param>
/// <returns>
/// The computed dot product.
/// </returns>
public float DotProduct(vec3f vector)
{
return(X * vector.X + Y * vector.Y + Z * vector.Z);
}
/// <summary>
/// Creates a new <c>AttitudeF</c> from yaw, pitch, roll in radians.
/// </summary>
/// <param name="ypr">
/// The yaw, pitch, roll in radians.
/// </param>
/// <returns>
/// The new <c>AttitudeF</c>.
/// </returns>
public static AttitudeF FromYprInRads(vec3f ypr)
{
return(new AttitudeF(AttitudeType.YprRads, ypr));
}
/// <summary>
/// Converts the input coordinates into speed over ground. Since the
/// input is a velocity a second set of coordinates is not needed.
/// </summary>
/// <param name="velNedX">
/// The X parameter of the velocity vector
/// </param>
/// <param name="velNedY">
/// The Y parameter of the velocity vector
/// </param>
/// <returns>
/// Computed speed over ground.
/// </returns>
public static float SpeedOverGround(vec3f velNed)
{
return(SpeedOverGround(velNed.X, velNed.Y));
}
/// <summary>
/// Converts the input vector into course over ground. Since the
/// input is a velocity a second vector is not needed.
/// </summary>
/// <param name="velNed">
/// The velocity vector
/// </param>
/// <returns>
/// Computed course over ground.
/// </returns>
public static float CourseOverGround(vec3f velNed)
{
return(CourseOverGround(velNed.X, velNed.Y));
}
/// <summary>
/// Converts an orientation represented as a yaw, pitch, roll in degrees to
/// a quaternion.
/// </summary>
/// <param name="ypr">
/// The yaw, pitch, roll values in degrees to convert.
/// </param>
/// <returns>
/// The corresponding quaternion.
/// </returns>
public static vec4f YprInDegs2Quat(vec3f ypr)
{
return(YprInRads2Quat(Deg2Rad(ypr)));
}
/// <summary>
/// Converts yaw, pitch, roll (degs) in the frame reported by a VectorNav
/// sensor to the appropriate Euler Angles used by Unity.
/// </summary>
/// <returns>
/// The converted values.
/// </returns>
/// <param name="yprInDegs">
/// Yaw, pitch, roll (degs) as reported by a VectorNav sensor.
/// </param>
public static Vector3 VnYprInDegsToUnityEulerAngles(vec3f yprInDegs)
{
return(new Vector3(-yprInDegs.Y, yprInDegs.X, -yprInDegs.Z));
}
/// <summary>
/// Converts a yaw, pitch, roll in radians to a direction cosine matrix.
/// </summary>
/// <param name="yprInDegs">
/// The yaw, pitch, roll rotation expressed in degrees.
/// </param>
/// <returns>
/// The corresponding direction cosine matrix.
/// </returns>
public static mat3f YprInDegs2Dcm(vec3f yprInDegs)
{
return(YprInRads2Dcm(Deg2Rad(yprInDegs)));
}
/// <summary>
/// Converts a collection of angles in degrees to radians.
/// </summary>
/// <param name="anglesInDegs">
/// The angles in degrees.
/// </param>
/// <returns>
/// The corresponding angles in radians.
/// </returns>
public static vec3f ToRad(this vec3f anglesInDegs)
{
return(Deg2Rad(anglesInDegs));
}
