public static bool AreEqualToTolerance(DoubleVector3 _vectorA, DoubleVector3 _vectorB, double _tolerance)
{
if (
EqualToTolerance(_vectorA.x, _vectorB.x, _tolerance) &&
EqualToTolerance(_vectorA.y, _vectorB.y, _tolerance) &&
EqualToTolerance(_vectorA.z, _vectorB.z, _tolerance)
)
{
return(true);
}
return(false);
}
/// <summary>
/// Converts an ECEF world position and a heading (direction you are facing) into a tangent basis
/// at that point, which can further be used to make calculations in that tangent space.
/// </summary>
/// <param name="worldPointEcef">World position in the ECEF system.</param>
/// <param name="absoluteHeadingDegrees">Absolute heading in degrees.</param>
public static EcefTangentBasis EcefTangentBasisFromPointAndHeading(DoubleVector3 worldPointEcef, float absoluteHeadingDegrees)
{
var heading = new Vector3(0.0f, 1.0f, 0.0f);
var tempBasis = new EcefTangentBasis();
tempBasis.Set(worldPointEcef, heading);
var up = tempBasis.Up;
var q = Quaternion.AngleAxis(absoluteHeadingDegrees, up);
heading = q.RotatePoint(heading);
var basis = new EcefTangentBasis();
basis.Set(worldPointEcef, heading);
return(basis);
}
/// <summary>
/// Create a basis using a surface ECEF point and a vector heading.
/// </summary>
/// <param name="pointEcef">Point on the surface of the earth in ECEF</param>
/// <param name="heading">A vector pointing along the forward direction</param>
public EcefTangentBasis(DoubleVector3 pointEcef, Vector3 heading)
{
Set(pointEcef, heading);
}
/// <summary>
/// Calculate a new tangent plane using the stored forward vector.
/// </summary>
/// <param name="pointEcef">The point on the surface of the earth in ECEF.</param>
public void SetPoint(DoubleVector3 pointEcef)
{
Set(pointEcef, m_basisForward);
}
/// <summary>
/// Calculate a new tangent plane and set its parameters in the tangent basis.
/// </summary>
/// <param name="pointEcef">The point on the surface of the earth in ECEF.</param>
/// <param name="heading">A vector pointing along the forward direction.</param>
public void Set(DoubleVector3 pointEcef, Vector3 heading)
{
CalculateTangentBasisVectorsFromPointAndHeading(pointEcef, heading, out m_basisRight, out m_basisUp, out m_basisForward);
m_pointEcef = pointEcef;
}
public static Vector3 ToSingleVector(this DoubleVector3 _vector)
{
return(new Vector3((float)_vector.x, (float)_vector.y, (float)_vector.z));
}