396 lines
13 KiB
C#
396 lines
13 KiB
C#
/*
|
|
* Author: Andrey Pokidov
|
|
* License: Apache-2.0
|
|
* Date: 1 Feb 2019
|
|
*/
|
|
|
|
namespace BGC
|
|
{
|
|
public struct Vector3FP64
|
|
{
|
|
public static readonly Vector3FP64 ZERO = new Vector3FP64(0.0, 0.0, 0.0);
|
|
|
|
public double x1 = 0.0;
|
|
public double x2 = 0.0;
|
|
public double x3 = 0.0;
|
|
|
|
public Vector3FP64(double x1, double x2, double x3)
|
|
{
|
|
this.x1 = x1;
|
|
this.x2 = x2;
|
|
this.x3 = x3;
|
|
}
|
|
|
|
public Vector3FP64(in Vector3FP64 vector)
|
|
{
|
|
this.x1 = vector.x1;
|
|
this.x2 = vector.x2;
|
|
this.x3 = vector.x3;
|
|
}
|
|
|
|
public Vector3FP64(in Vector3FP32 vector)
|
|
{
|
|
this.x1 = vector.x1;
|
|
this.x2 = vector.x2;
|
|
this.x3 = vector.x3;
|
|
}
|
|
|
|
public readonly double GetSquareModulus()
|
|
{
|
|
return this.x1 * this.x1 + this.x2 * this.x2 + this.x3 * this.x3;
|
|
}
|
|
|
|
public readonly double GetModulus()
|
|
{
|
|
return Math.Sqrt(this.GetSquareModulus());
|
|
}
|
|
|
|
public readonly bool IsZero()
|
|
{
|
|
return this.GetSquareModulus() <= UtilityFP64.SQUARE_EPSYLON;
|
|
}
|
|
|
|
public readonly bool IsUnit()
|
|
{
|
|
return UtilityFP64.IsSqareUnit(this.GetSquareModulus());
|
|
}
|
|
|
|
public void Reset()
|
|
{
|
|
this.x1 = 0.0;
|
|
this.x2 = 0.0;
|
|
this.x3 = 0.0;
|
|
}
|
|
|
|
public void MakeOpposite()
|
|
{
|
|
this.x1 = -this.x1;
|
|
this.x2 = -this.x2;
|
|
this.x3 = -this.x3;
|
|
}
|
|
|
|
public readonly Vector3FP64 GetOpposite()
|
|
{
|
|
return new Vector3FP64(-this.x1, -this.x2, -this.x3);
|
|
}
|
|
|
|
public bool Normalize()
|
|
{
|
|
double squareModulus = this.GetSquareModulus();
|
|
|
|
if (UtilityFP64.IsSqareUnit(squareModulus))
|
|
{
|
|
return true;
|
|
}
|
|
|
|
if (squareModulus <= UtilityFP64.SQUARE_EPSYLON || double.IsNaN(squareModulus))
|
|
{
|
|
return false;
|
|
}
|
|
|
|
double multiplier = Math.Sqrt(1.0 / squareModulus);
|
|
|
|
this.x1 *= multiplier;
|
|
this.x2 *= multiplier;
|
|
this.x3 *= multiplier;
|
|
|
|
return true;
|
|
}
|
|
|
|
public void SetValues(double x1, double x2, double x3)
|
|
{
|
|
this.x1 = x1;
|
|
this.x2 = x2;
|
|
this.x3 = x3;
|
|
}
|
|
|
|
public void SetValues(in Vector3FP64 vector)
|
|
{
|
|
this.x1 = vector.x1;
|
|
this.x2 = vector.x2;
|
|
this.x3 = vector.x3;
|
|
}
|
|
|
|
public void SetValues(in Vector3FP32 vector)
|
|
{
|
|
this.x1 = vector.x1;
|
|
this.x2 = vector.x2;
|
|
this.x3 = vector.x3;
|
|
}
|
|
|
|
public readonly override string ToString()
|
|
{
|
|
return String.Format("Vector3FP64({0}, {1}, {2})", this.x1, this.x2, this.x3);
|
|
}
|
|
|
|
public static void Swap(ref Vector3FP64 vector1, ref Vector3FP64 vector2)
|
|
{
|
|
double x1 = vector1.x1;
|
|
double x2 = vector1.x2;
|
|
double x3 = vector1.x3;
|
|
|
|
vector1.x1 = vector2.x1;
|
|
vector1.x2 = vector2.x2;
|
|
vector1.x3 = vector2.x3;
|
|
|
|
vector2.x1 = x1;
|
|
vector2.x2 = x2;
|
|
vector2.x3 = x3;
|
|
}
|
|
|
|
public static void Reset(out Vector3FP64 vector)
|
|
{
|
|
vector.x1 = 0.0;
|
|
vector.x2 = 0.0;
|
|
vector.x3 = 0.0;
|
|
}
|
|
|
|
public static void GetOpposite(in Vector3FP64 vector, out Vector3FP64 reverted)
|
|
{
|
|
reverted.x1 = -vector.x1;
|
|
reverted.x2 = -vector.x2;
|
|
reverted.x3 = -vector.x3;
|
|
}
|
|
|
|
public static bool GetNormalized(in Vector3FP64 vector, out Vector3FP64 normalized)
|
|
{
|
|
double squareModulus = vector.GetSquareModulus();
|
|
|
|
if (UtilityFP64.IsSqareUnit(squareModulus))
|
|
{
|
|
normalized.x1 = vector.x1;
|
|
normalized.x2 = vector.x2;
|
|
normalized.x3 = vector.x3;
|
|
return true;
|
|
}
|
|
|
|
if (squareModulus <= UtilityFP64.SQUARE_EPSYLON || double.IsNaN(squareModulus))
|
|
{
|
|
normalized.x1 = 0.0;
|
|
normalized.x2 = 0.0;
|
|
normalized.x3 = 0.0;
|
|
return false;
|
|
}
|
|
|
|
double multiplier = Math.Sqrt(1.0 / squareModulus);
|
|
|
|
normalized.x1 = vector.x1 * multiplier;
|
|
normalized.x2 = vector.x2 * multiplier;
|
|
normalized.x3 = vector.x3 * multiplier;
|
|
return true;
|
|
}
|
|
|
|
public static void Add(in Vector3FP64 vector1, in Vector3FP64 vector2, out Vector3FP64 sum)
|
|
{
|
|
sum.x1 = vector1.x1 + vector2.x1;
|
|
sum.x2 = vector1.x2 + vector2.x2;
|
|
sum.x3 = vector1.x3 + vector2.x3;
|
|
}
|
|
|
|
public static void AddScaled(in Vector3FP64 basicVector, in Vector3FP64 scalableVector, double scale, out Vector3FP64 sum)
|
|
{
|
|
sum.x1 = basicVector.x1 + scalableVector.x1 * scale;
|
|
sum.x2 = basicVector.x2 + scalableVector.x2 * scale;
|
|
sum.x3 = basicVector.x3 + scalableVector.x3 * scale;
|
|
}
|
|
|
|
public static void Subtract(in Vector3FP64 minuend, in Vector3FP64 subtrahend, out Vector3FP64 difference)
|
|
{
|
|
difference.x1 = minuend.x1 - subtrahend.x1;
|
|
difference.x2 = minuend.x2 - subtrahend.x2;
|
|
difference.x3 = minuend.x3 - subtrahend.x3;
|
|
}
|
|
|
|
public static void Multiply(in Vector3FP64 multiplicand, double multiplier, out Vector3FP64 product)
|
|
{
|
|
product.x1 = multiplicand.x1 * multiplier;
|
|
product.x2 = multiplicand.x2 * multiplier;
|
|
product.x3 = multiplicand.x3 * multiplier;
|
|
}
|
|
|
|
public static void Divide(in Vector3FP64 dividend, double divisor, out Vector3FP64 quotient)
|
|
{
|
|
Multiply(dividend, 1.0 / divisor, out quotient);
|
|
}
|
|
|
|
public static void GetMeanOfTwo(in Vector3FP64 vector1, in Vector3FP64 vector2, out Vector3FP64 mean)
|
|
{
|
|
mean.x1 = (vector1.x1 + vector2.x1) * 0.5;
|
|
mean.x2 = (vector1.x2 + vector2.x2) * 0.5;
|
|
mean.x3 = (vector1.x3 + vector2.x3) * 0.5;
|
|
}
|
|
|
|
public static void GetMeanOfThree(in Vector3FP64 vector1, in Vector3FP64 vector2, in Vector3FP64 vector3, out Vector3FP64 mean)
|
|
{
|
|
mean.x1 = (vector1.x1 + vector2.x1 + vector3.x1) * UtilityFP64.ONE_THIRD;
|
|
mean.x2 = (vector1.x2 + vector2.x2 + vector3.x2) * UtilityFP64.ONE_THIRD;
|
|
mean.x3 = (vector1.x3 + vector2.x3 + vector3.x3) * UtilityFP64.ONE_THIRD;
|
|
}
|
|
|
|
public static void Interpolate(in Vector3FP64 vector1, in Vector3FP64 vector2, double phase, out Vector3FP64 interpolation)
|
|
{
|
|
double counterphase = 1.0 - phase;
|
|
|
|
interpolation.x1 = vector1.x1 * counterphase + vector2.x1 * phase;
|
|
interpolation.x2 = vector1.x2 * counterphase + vector2.x2 * phase;
|
|
interpolation.x3 = vector1.x3 * counterphase + vector2.x3 * phase;
|
|
}
|
|
|
|
public static double GetScalarProduct(in Vector3FP64 vector1, in Vector3FP64 vector2)
|
|
{
|
|
return vector1.x1 * vector2.x1 + vector1.x2 * vector2.x2 + vector1.x3 * vector2.x3;
|
|
}
|
|
|
|
public static double GetTripleProduct(in Vector3FP64 vector1, in Vector3FP64 vector2, in Vector3FP64 vector3)
|
|
{
|
|
return vector1.x1 * (vector2.x2 * vector3.x3 - vector2.x3 * vector3.x2)
|
|
+ vector1.x2 * (vector2.x3 * vector3.x1 - vector2.x1 * vector3.x3)
|
|
+ vector1.x3 * (vector2.x1 * vector3.x2 - vector2.x2 * vector3.x1);
|
|
}
|
|
|
|
public static void GetCrossProduct(in Vector3FP64 vector1, in Vector3FP64 vector2, out Vector3FP64 result)
|
|
{
|
|
double x1 = vector1.x2 * vector2.x3 - vector1.x3 * vector2.x2;
|
|
double x2 = vector1.x3 * vector2.x1 - vector1.x1 * vector2.x3;
|
|
double x3 = vector1.x1 * vector2.x2 - vector1.x2 * vector2.x1;
|
|
|
|
result.x1 = x1;
|
|
result.x2 = x2;
|
|
result.x3 = x3;
|
|
}
|
|
|
|
// [a x [b x c]] = b * (a, c) - c * (a, b)
|
|
public static void GetDoubleCrossProduct(in Vector3FP64 vector1, in Vector3FP64 vector2, in Vector3FP64 vector3, out Vector3FP64 product)
|
|
{
|
|
double ac = GetScalarProduct(vector1, vector3);
|
|
double ab = GetScalarProduct(vector1, vector2);
|
|
|
|
product.x1 = ac * vector2.x1 - ab * vector3.x1;
|
|
product.x2 = ac * vector2.x2 - ab * vector3.x2;
|
|
product.x3 = ac * vector2.x3 - ab * vector3.x3;
|
|
}
|
|
|
|
public static double GetAngle(in Vector3FP64 vector1, in Vector3FP64 vector2, AngleUnit unit)
|
|
{
|
|
double squareModulus1 = vector1.GetSquareModulus();
|
|
|
|
if (squareModulus1 <= UtilityFP64.SQUARE_EPSYLON || double.IsNaN(squareModulus1))
|
|
{
|
|
return 0.0f;
|
|
}
|
|
|
|
double squareModulus2 = vector2.GetSquareModulus();
|
|
|
|
if (squareModulus2 <= UtilityFP64.SQUARE_EPSYLON || double.IsNaN(squareModulus2))
|
|
{
|
|
return 0.0f;
|
|
}
|
|
|
|
double multiplier = Math.Sqrt(1.0 / (squareModulus1 * squareModulus2));
|
|
|
|
Vector3FP64 crossProduct;
|
|
|
|
GetCrossProduct(vector1, vector2, out crossProduct);
|
|
|
|
double x = GetScalarProduct(vector1, vector2);
|
|
double y = crossProduct.GetModulus();
|
|
|
|
return RadianFP64.ToUnits(Math.Atan2(y * multiplier, x * multiplier), unit);
|
|
}
|
|
|
|
public static double GetSquareDistance(in Vector3FP64 vector1, in Vector3FP64 vector2)
|
|
{
|
|
double dx1 = vector1.x1 - vector2.x1;
|
|
double dx2 = vector1.x2 - vector2.x2;
|
|
double dx3 = vector1.x3 - vector2.x3;
|
|
|
|
return dx1 * dx1 + dx2 * dx2 + dx3 * dx3;
|
|
}
|
|
|
|
public static double GetDistance(in Vector3FP64 vector1, in Vector3FP64 vector2)
|
|
{
|
|
return Math.Sqrt(GetSquareDistance(vector1, vector2));
|
|
}
|
|
|
|
public static bool AreCloseEnough(in Vector3FP64 vector1, in Vector3FP64 vector2, double distanceLimit)
|
|
{
|
|
return 0.0 <= distanceLimit && GetSquareDistance(vector1, vector2) <= distanceLimit * distanceLimit;
|
|
}
|
|
|
|
public static bool AreClose(in Vector3FP64 vector1, in Vector3FP64 vector2)
|
|
{
|
|
double squareModulus1 = vector1.GetSquareModulus();
|
|
double squareModulus2 = vector2.GetSquareModulus();
|
|
double squareDistance = GetSquareDistance(vector1, vector2);
|
|
|
|
if (squareModulus1 <= UtilityFP64.EPSYLON_EFFECTIVENESS_LIMIT || squareModulus2 <= UtilityFP64.EPSYLON_EFFECTIVENESS_LIMIT)
|
|
{
|
|
return squareDistance <= UtilityFP64.SQUARE_EPSYLON;
|
|
}
|
|
|
|
return squareDistance <= UtilityFP64.SQUARE_EPSYLON * squareModulus1 && squareDistance <= UtilityFP64.SQUARE_EPSYLON * squareModulus2;
|
|
}
|
|
|
|
public static bool AreParallel(in Vector3FP64 vector1, in Vector3FP64 vector2)
|
|
{
|
|
double squareModulus1 = vector1.GetSquareModulus();
|
|
double squareModulus2 = vector2.GetSquareModulus();
|
|
|
|
if (squareModulus1 <= UtilityFP64.SQUARE_EPSYLON || squareModulus2 <= UtilityFP64.SQUARE_EPSYLON)
|
|
{
|
|
return true;
|
|
}
|
|
|
|
Vector3FP64 crossProduct;
|
|
|
|
GetCrossProduct(vector1, vector2, out crossProduct);
|
|
|
|
return crossProduct.GetSquareModulus() <= UtilityFP64.SQUARE_EPSYLON * squareModulus1 * squareModulus2;
|
|
}
|
|
|
|
public static bool AreOrthogonal(in Vector3FP64 vector1, in Vector3FP64 vector2)
|
|
{
|
|
double squareModulus1 = vector1.GetSquareModulus();
|
|
double squareModulus2 = vector2.GetSquareModulus();
|
|
|
|
if (squareModulus1 <= UtilityFP64.SQUARE_EPSYLON || squareModulus2 <= UtilityFP64.SQUARE_EPSYLON)
|
|
{
|
|
return true;
|
|
}
|
|
|
|
double scalarProduct = GetScalarProduct(vector1, vector2);
|
|
|
|
return scalarProduct * scalarProduct <= UtilityFP64.SQUARE_EPSYLON * squareModulus1 * squareModulus2;
|
|
}
|
|
|
|
public static Attitude GetAttitude(in Vector3FP64 vector1, in Vector3FP64 vector2)
|
|
{
|
|
double squareModulus1 = vector1.GetSquareModulus();
|
|
double squareModulus2 = vector2.GetSquareModulus();
|
|
|
|
if (squareModulus1 <= UtilityFP64.SQUARE_EPSYLON || squareModulus2 <= UtilityFP64.SQUARE_EPSYLON)
|
|
{
|
|
return Attitude.ZERO;
|
|
}
|
|
|
|
double squareLimit = UtilityFP64.SQUARE_EPSYLON * squareModulus1 * squareModulus2;
|
|
double scalarProduct = GetScalarProduct(vector1, vector2);
|
|
|
|
if (scalarProduct * scalarProduct <= squareLimit)
|
|
{
|
|
return Attitude.ORTHOGONAL;
|
|
}
|
|
|
|
Vector3FP64 crossProduct;
|
|
|
|
GetCrossProduct(vector1, vector2, out crossProduct);
|
|
|
|
if (crossProduct.GetSquareModulus() > squareLimit)
|
|
{
|
|
return Attitude.ANY;
|
|
}
|
|
|
|
return scalarProduct > 0.0f ? Attitude.CO_DIRECTIONAL : Attitude.COUNTER_DIRECTIONAL;
|
|
}
|
|
}
|
|
}
|