/// <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));
 }
Esempio n. 4
0
 /// <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)));
 }
Esempio n. 5
0
 /// <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)));
 }
Esempio n. 6
0
        /// <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));
        }
Esempio n. 7
0
        /// <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));
 }
Esempio n. 10
0
 /// <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));
 }
Esempio n. 11
0
 /// <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));
 }
Esempio n. 12
0
 /// <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)));
 }
Esempio n. 13
0
 /// <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));
 }
Esempio n. 14
0
 /// <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)));
 }
Esempio n. 15
0
 /// <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));
 }