Упорядочивание проекта и синхронизация с BGC-C
This commit is contained in:
parent
a072da442e
commit
791fda6a7f
38 changed files with 1838 additions and 650 deletions
|
|
@ -4,7 +4,7 @@
|
|||
* Date: 1 Feb 2019
|
||||
*/
|
||||
|
||||
namespace BasicGeometry
|
||||
namespace BGC
|
||||
{
|
||||
public enum AngleUnit
|
||||
{
|
||||
|
|
|
|||
|
|
@ -4,7 +4,7 @@
|
|||
* Date: 1 Feb 2019
|
||||
*/
|
||||
|
||||
namespace BasicGeometry
|
||||
namespace BGC
|
||||
{
|
||||
public static class AngleFP32
|
||||
{
|
||||
|
|
|
|||
|
|
@ -4,7 +4,7 @@
|
|||
* Date: 1 Feb 2019
|
||||
*/
|
||||
|
||||
namespace BasicGeometry
|
||||
namespace BGC
|
||||
{
|
||||
public static class AngleFP64
|
||||
{
|
||||
|
|
|
|||
11
BasicGeometry/Attitude.cs
Normal file
11
BasicGeometry/Attitude.cs
Normal file
|
|
@ -0,0 +1,11 @@
|
|||
namespace BGC
|
||||
{
|
||||
public enum Attitude
|
||||
{
|
||||
ANY = 0,
|
||||
ZERO = 1,
|
||||
ORTHOGONAL = 2,
|
||||
CO_DIRECTIONAL = 3,
|
||||
COUNTER_DIRECTIONAL = 4
|
||||
}
|
||||
}
|
||||
12
BasicGeometry/Axis.cs
Normal file
12
BasicGeometry/Axis.cs
Normal file
|
|
@ -0,0 +1,12 @@
|
|||
namespace BGC
|
||||
{
|
||||
public enum Axis
|
||||
{
|
||||
X1 = 1,
|
||||
X2 = 2,
|
||||
X3 = 3,
|
||||
REVERSE_X1 = -1,
|
||||
REVERSE_X2 = -2,
|
||||
REVERSE_X3 = -3
|
||||
}
|
||||
}
|
||||
171
BasicGeometry/ComplexFP32.cs
Normal file
171
BasicGeometry/ComplexFP32.cs
Normal file
|
|
@ -0,0 +1,171 @@
|
|||
|
||||
namespace BGC
|
||||
{
|
||||
public struct ComplexFP32
|
||||
{
|
||||
public float real = 0.0f;
|
||||
public float imaginary = 0.0f;
|
||||
|
||||
public ComplexFP32(float real, float imaginary)
|
||||
{
|
||||
this.real = real;
|
||||
this.imaginary = imaginary;
|
||||
}
|
||||
|
||||
public ComplexFP32(in ComplexFP32 number)
|
||||
{
|
||||
this.real = number.real;
|
||||
this.imaginary = number.imaginary;
|
||||
}
|
||||
|
||||
public readonly float GetSquareModulus()
|
||||
{
|
||||
return this.real * this.real + this.imaginary * this.imaginary;
|
||||
}
|
||||
|
||||
public readonly float GetModulus()
|
||||
{
|
||||
return MathF.Sqrt(GetSquareModulus());
|
||||
}
|
||||
|
||||
public readonly bool IsZero()
|
||||
{
|
||||
return GetSquareModulus() <= UtilityFP32.SQUARE_EPSYLON;
|
||||
}
|
||||
|
||||
public readonly bool IsUnit()
|
||||
{
|
||||
return UtilityFP32.IsSqareUnit(GetSquareModulus());
|
||||
}
|
||||
|
||||
public void Reset()
|
||||
{
|
||||
this.real = 0.0f;
|
||||
this.imaginary = 0.0f;
|
||||
}
|
||||
|
||||
public void MakeOpposite()
|
||||
{
|
||||
this.real = -this.real;
|
||||
this.imaginary = -this.imaginary;
|
||||
}
|
||||
|
||||
public bool Normalize()
|
||||
{
|
||||
float squareModulus = this.GetSquareModulus();
|
||||
|
||||
if (UtilityFP32.IsSqareUnit(squareModulus))
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
if (squareModulus <= UtilityFP32.SQUARE_EPSYLON || float.IsNaN(squareModulus))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
float multiplier = MathF.Sqrt(1.0f / squareModulus);
|
||||
|
||||
this.real *= multiplier;
|
||||
this.imaginary *= multiplier;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
public void Conjugate()
|
||||
{
|
||||
this.imaginary = -this.imaginary;
|
||||
}
|
||||
|
||||
public bool Invert()
|
||||
{
|
||||
float squareModulus = this.GetSquareModulus();
|
||||
|
||||
if (squareModulus <= UtilityFP32.SQUARE_EPSYLON || float.IsNaN(squareModulus))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
float multiplicand = 1.0f / squareModulus;
|
||||
|
||||
this.real *= multiplicand;
|
||||
this.imaginary = -this.imaginary * multiplicand;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
public void SetValues(float real, float imaginary)
|
||||
{
|
||||
this.real = real;
|
||||
this.imaginary = imaginary;
|
||||
}
|
||||
|
||||
public void SetValues(in ComplexFP32 number)
|
||||
{
|
||||
this.real = number.real;
|
||||
this.imaginary = number.imaginary;
|
||||
}
|
||||
|
||||
public static void Reset(out ComplexFP32 number)
|
||||
{
|
||||
number.real = 0.0f;
|
||||
number.imaginary = 0.0f;
|
||||
}
|
||||
|
||||
public static void GetOpposite(in ComplexFP32 number, out ComplexFP32 opposite)
|
||||
{
|
||||
opposite.real = -number.real;
|
||||
opposite.imaginary = -number.imaginary;
|
||||
}
|
||||
|
||||
public static bool GetNormalized(in ComplexFP32 number, out ComplexFP32 normalized)
|
||||
{
|
||||
float squareModulus = number.GetSquareModulus();
|
||||
|
||||
if (UtilityFP64.IsSqareUnit(squareModulus))
|
||||
{
|
||||
normalized.real = number.real;
|
||||
normalized.imaginary = number.imaginary;
|
||||
return true;
|
||||
}
|
||||
|
||||
if (squareModulus <= UtilityFP64.SQUARE_EPSYLON || float.IsNaN(squareModulus))
|
||||
{
|
||||
normalized.real = 0.0f;
|
||||
normalized.imaginary = 0.0f;
|
||||
return false;
|
||||
}
|
||||
|
||||
float multiplier = MathF.Sqrt(1.0f / squareModulus);
|
||||
|
||||
normalized.real = number.real * multiplier;
|
||||
normalized.imaginary = number.imaginary * multiplier;
|
||||
return true;
|
||||
}
|
||||
|
||||
public static void GetConjugate(in ComplexFP32 number, out ComplexFP32 conjugate)
|
||||
{
|
||||
conjugate.real = number.real;
|
||||
conjugate.imaginary = -number.imaginary;
|
||||
}
|
||||
|
||||
public static bool GetInverse(in ComplexFP32 number, out ComplexFP32 inverse)
|
||||
{
|
||||
float squareModulus = number.GetSquareModulus();
|
||||
|
||||
if (squareModulus <= UtilityFP32.SQUARE_EPSYLON || float.IsNaN(squareModulus))
|
||||
{
|
||||
inverse.real = 0.0f;
|
||||
inverse.imaginary = 0.0f;
|
||||
return false;
|
||||
}
|
||||
|
||||
float multiplicand = 1.0f / squareModulus;
|
||||
|
||||
inverse.real = number.real * multiplicand;
|
||||
inverse.imaginary = -number.imaginary * multiplicand;
|
||||
|
||||
return true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -4,7 +4,7 @@
|
|||
* Date: 18 Nov 2024
|
||||
*/
|
||||
|
||||
namespace BasicGeometry
|
||||
namespace BGC
|
||||
{
|
||||
public class DegreeFP32
|
||||
{
|
||||
|
|
|
|||
|
|
@ -4,7 +4,7 @@
|
|||
* Date: 18 Nov 2024
|
||||
*/
|
||||
|
||||
namespace BasicGeometry
|
||||
namespace BGC
|
||||
{
|
||||
public class DegreeFP64
|
||||
{
|
||||
|
|
|
|||
|
|
@ -4,7 +4,7 @@
|
|||
* Date: 10 Feb 2019
|
||||
*/
|
||||
|
||||
namespace BasicGeometry
|
||||
namespace BGC
|
||||
{
|
||||
public struct Matrix2x2FP32
|
||||
{
|
||||
|
|
|
|||
|
|
@ -3,7 +3,7 @@
|
|||
* Date: 10 Feb 2019
|
||||
*/
|
||||
|
||||
namespace BasicGeometry
|
||||
namespace BGC
|
||||
{
|
||||
public struct Matrix2x2FP64
|
||||
{
|
||||
|
|
|
|||
|
|
@ -3,7 +3,7 @@
|
|||
* License: Apache-2.0
|
||||
* Date: 11 Nov 2024
|
||||
*/
|
||||
namespace BasicGeometry
|
||||
namespace BGC
|
||||
{
|
||||
public struct Matrix2x3FP32
|
||||
{
|
||||
|
|
|
|||
|
|
@ -3,7 +3,7 @@
|
|||
* License: Apache-2.0
|
||||
* Date: 11 Nov 2024
|
||||
*/
|
||||
namespace BasicGeometry
|
||||
namespace BGC
|
||||
{
|
||||
public struct Matrix2x3FP64
|
||||
{
|
||||
|
|
|
|||
|
|
@ -3,7 +3,7 @@
|
|||
* License: Apache-2.0
|
||||
* Date: 11 Nov 2024
|
||||
*/
|
||||
namespace BasicGeometry
|
||||
namespace BGC
|
||||
{
|
||||
public struct Matrix3x2FP32
|
||||
{
|
||||
|
|
|
|||
|
|
@ -3,7 +3,7 @@
|
|||
* License: Apache-2.0
|
||||
* Date: 11 Nov 2024
|
||||
*/
|
||||
namespace BasicGeometry
|
||||
namespace BGC
|
||||
{
|
||||
public struct Matrix3x2FP64
|
||||
{
|
||||
|
|
|
|||
|
|
@ -4,7 +4,7 @@
|
|||
* Date: 10 Feb 2019
|
||||
*/
|
||||
|
||||
namespace BasicGeometry
|
||||
namespace BGC
|
||||
{
|
||||
public struct Matrix3x3FP32
|
||||
{
|
||||
|
|
@ -223,7 +223,7 @@ namespace BasicGeometry
|
|||
this.r3c3 = r3;
|
||||
}
|
||||
|
||||
public static void MakeTransposed(in Matrix3x3FP64 matrix, out Matrix3x3FP64 transposed)
|
||||
public static void MakeTransposed(in Matrix3x3FP32 matrix, out Matrix3x3FP32 transposed)
|
||||
{
|
||||
transposed.r1c1 = matrix.r1c1;
|
||||
transposed.r2c2 = matrix.r2c2;
|
||||
|
|
@ -317,21 +317,6 @@ namespace BasicGeometry
|
|||
difference.r3c3 = minuend.r3c3 - subtrahend.r3c3;
|
||||
}
|
||||
|
||||
public static void SubtractScaled(in Matrix3x3FP32 basicMatrix, in Matrix3x3FP32 scalableMatrix, float scale, out Matrix3x3FP32 difference)
|
||||
{
|
||||
difference.r1c1 = basicMatrix.r1c1 - scalableMatrix.r1c1 * scale;
|
||||
difference.r1c2 = basicMatrix.r1c2 - scalableMatrix.r1c2 * scale;
|
||||
difference.r1c3 = basicMatrix.r1c3 - scalableMatrix.r1c3 * scale;
|
||||
|
||||
difference.r2c1 = basicMatrix.r2c1 - scalableMatrix.r2c1 * scale;
|
||||
difference.r2c2 = basicMatrix.r2c2 - scalableMatrix.r2c2 * scale;
|
||||
difference.r2c3 = basicMatrix.r2c3 - scalableMatrix.r2c3 * scale;
|
||||
|
||||
difference.r3c1 = basicMatrix.r3c1 - scalableMatrix.r3c1 * scale;
|
||||
difference.r3c2 = basicMatrix.r3c2 - scalableMatrix.r3c2 * scale;
|
||||
difference.r3c3 = basicMatrix.r3c3 - scalableMatrix.r3c3 * scale;
|
||||
}
|
||||
|
||||
public static void Multiply(in Matrix3x3FP32 multiplicand, float multiplier, out Matrix3x3FP32 product)
|
||||
{
|
||||
product.r1c1 = multiplicand.r1c1 * multiplier;
|
||||
|
|
|
|||
|
|
@ -4,7 +4,7 @@
|
|||
* Date: 10 Feb 2019
|
||||
*/
|
||||
|
||||
namespace BasicGeometry
|
||||
namespace BGC
|
||||
{
|
||||
public struct Matrix3x3FP64
|
||||
{
|
||||
|
|
@ -223,7 +223,7 @@ namespace BasicGeometry
|
|||
this.r3c3 = r3;
|
||||
}
|
||||
|
||||
public static void MakeTransposed(in Matrix3x3FP64 matrix, out Matrix3x3FP64 transposed)
|
||||
public static void GetTransposed(in Matrix3x3FP64 matrix, out Matrix3x3FP64 transposed)
|
||||
{
|
||||
transposed.r1c1 = matrix.r1c1;
|
||||
transposed.r2c2 = matrix.r2c2;
|
||||
|
|
@ -234,7 +234,7 @@ namespace BasicGeometry
|
|||
(transposed.r2c3, transposed.r3c2) = (matrix.r3c2, matrix.r2c3);
|
||||
}
|
||||
|
||||
public static bool MakeInverted(in Matrix3x3FP64 matrix, out Matrix3x3FP64 inverted)
|
||||
public static bool GetInverted(in Matrix3x3FP64 matrix, out Matrix3x3FP64 inverted)
|
||||
{
|
||||
double determinant = matrix.GetDeterminant();
|
||||
|
||||
|
|
@ -317,21 +317,6 @@ namespace BasicGeometry
|
|||
difference.r3c3 = minuend.r3c3 - subtrahend.r3c3;
|
||||
}
|
||||
|
||||
public static void SubtractScaled(in Matrix3x3FP64 basicMatrix, in Matrix3x3FP64 scalableMatrix, double scale, out Matrix3x3FP64 difference)
|
||||
{
|
||||
difference.r1c1 = basicMatrix.r1c1 - scalableMatrix.r1c1 * scale;
|
||||
difference.r1c2 = basicMatrix.r1c2 - scalableMatrix.r1c2 * scale;
|
||||
difference.r1c3 = basicMatrix.r1c3 - scalableMatrix.r1c3 * scale;
|
||||
|
||||
difference.r2c1 = basicMatrix.r2c1 - scalableMatrix.r2c1 * scale;
|
||||
difference.r2c2 = basicMatrix.r2c2 - scalableMatrix.r2c2 * scale;
|
||||
difference.r2c3 = basicMatrix.r2c3 - scalableMatrix.r2c3 * scale;
|
||||
|
||||
difference.r3c1 = basicMatrix.r3c1 - scalableMatrix.r3c1 * scale;
|
||||
difference.r3c2 = basicMatrix.r3c2 - scalableMatrix.r3c2 * scale;
|
||||
difference.r3c3 = basicMatrix.r3c3 - scalableMatrix.r3c3 * scale;
|
||||
}
|
||||
|
||||
public static void Multiply(in Matrix3x3FP64 multiplicand, double multiplier, out Matrix3x3FP64 product)
|
||||
{
|
||||
product.r1c1 = multiplicand.r1c1 * multiplier;
|
||||
|
|
|
|||
|
|
@ -4,7 +4,7 @@
|
|||
* Date: 11 Nov 2024
|
||||
*/
|
||||
|
||||
namespace BasicGeometry
|
||||
namespace BGC
|
||||
{
|
||||
public class MatrixProductFP32
|
||||
{
|
||||
|
|
|
|||
|
|
@ -4,7 +4,7 @@
|
|||
* Date: 11 Nov 2024
|
||||
*/
|
||||
|
||||
namespace BasicGeometry
|
||||
namespace BGC
|
||||
{
|
||||
public class MatrixProductFP64
|
||||
{
|
||||
|
|
|
|||
|
|
@ -4,7 +4,7 @@
|
|||
* Date: 4 Dec 2024
|
||||
*/
|
||||
|
||||
namespace BasicGeometry
|
||||
namespace BGC
|
||||
{
|
||||
public struct QuaternionFP32
|
||||
{
|
||||
|
|
@ -62,7 +62,7 @@ namespace BasicGeometry
|
|||
this.x3 = 0.0f;
|
||||
}
|
||||
|
||||
public void SetToIdentity()
|
||||
public void MakeUnit()
|
||||
{
|
||||
this.s0 = 1.0f;
|
||||
this.x1 = 0.0f;
|
||||
|
|
@ -77,6 +77,52 @@ namespace BasicGeometry
|
|||
this.x3 = -this.x3;
|
||||
}
|
||||
|
||||
public void MakeOpposit()
|
||||
{
|
||||
this.s0 = -this.s0;
|
||||
this.x1 = -this.x1;
|
||||
this.x2 = -this.x2;
|
||||
this.x3 = -this.x3;
|
||||
}
|
||||
|
||||
public bool Invert()
|
||||
{
|
||||
float squareModulus = this.GetSquareModulus();
|
||||
|
||||
if (squareModulus <= UtilityFP32.SQUARE_EPSYLON || float.IsNaN(squareModulus))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
float multiplicand = 1.0f / squareModulus;
|
||||
|
||||
this.s0 = this.s0 * multiplicand;
|
||||
this.x1 = -this.x1 * multiplicand;
|
||||
this.x2 = -this.x2 * multiplicand;
|
||||
this.x3 = -this.x3 * multiplicand;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
public bool Normalize()
|
||||
{
|
||||
float squareModulus = this.GetSquareModulus();
|
||||
|
||||
if (squareModulus <= UtilityFP32.SQUARE_EPSYLON || float.IsNaN(squareModulus))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
float multiplier = MathF.Sqrt(1.0f / squareModulus);
|
||||
|
||||
this.s0 *= multiplier;
|
||||
this.x1 *= multiplier;
|
||||
this.x2 *= multiplier;
|
||||
this.x3 *= multiplier;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
public void SetValues(float s0, float x1, float x2, float x3)
|
||||
{
|
||||
this.s0 = s0;
|
||||
|
|
@ -85,7 +131,7 @@ namespace BasicGeometry
|
|||
this.x3 = x3;
|
||||
}
|
||||
|
||||
public void Set(in QuaternionFP32 quaternion)
|
||||
public void SetValues(in QuaternionFP32 quaternion)
|
||||
{
|
||||
this.s0 = quaternion.s0;
|
||||
this.x1 = quaternion.x1;
|
||||
|
|
@ -101,90 +147,38 @@ namespace BasicGeometry
|
|||
this.x3 = (float)quaternion.x3;
|
||||
}
|
||||
|
||||
public static void MakeConjugate(in QuaternionFP32 quaternion, out QuaternionFP32 conjugate)
|
||||
public static void Reset(out QuaternionFP32 quaternion)
|
||||
{
|
||||
conjugate.s0 = quaternion.s0;
|
||||
conjugate.x1 = -quaternion.x1;
|
||||
conjugate.x2 = -quaternion.x2;
|
||||
conjugate.x3 = -quaternion.x3;
|
||||
quaternion.s0 = 0.0f;
|
||||
quaternion.x1 = 0.0f;
|
||||
quaternion.x2 = 0.0f;
|
||||
quaternion.x3 = 0.0f;
|
||||
}
|
||||
|
||||
public readonly void MakeRotationMatrix(out Matrix3x3FP32 matrix)
|
||||
public static void MakeUnit(out QuaternionFP32 quaternion)
|
||||
{
|
||||
float s0s0 = this.s0 * this.s0;
|
||||
float x1x1 = this.x1 * this.x1;
|
||||
float x2x2 = this.x2 * this.x2;
|
||||
float x3x3 = this.x3 * this.x3;
|
||||
|
||||
float squareModulus = (s0s0 + x1x1) + (x2x2 + x3x3);
|
||||
|
||||
if (squareModulus <= UtilityFP32.SQUARE_EPSYLON)
|
||||
{
|
||||
Matrix3x3FP32.LoadIdentity(out matrix);
|
||||
return;
|
||||
}
|
||||
|
||||
float corrector1 = 1.0f / squareModulus;
|
||||
|
||||
float s0x1 = this.s0 * this.x1;
|
||||
float s0x2 = this.s0 * this.x2;
|
||||
float s0x3 = this.s0 * this.x3;
|
||||
float x1x2 = this.x1 * this.x2;
|
||||
float x1x3 = this.x1 * this.x3;
|
||||
float x2x3 = this.x2 * this.x3;
|
||||
|
||||
float corrector2 = 2.0f * corrector1;
|
||||
|
||||
matrix.r1c1 = corrector1 * ((s0s0 + x1x1) - (x2x2 + x3x3));
|
||||
matrix.r2c2 = corrector1 * ((s0s0 + x2x2) - (x1x1 + x3x3));
|
||||
matrix.r3c3 = corrector1 * ((s0s0 + x3x3) - (x1x1 + x2x2));
|
||||
|
||||
matrix.r1c2 = corrector2 * (x1x2 - s0x3);
|
||||
matrix.r2c3 = corrector2 * (x2x3 - s0x1);
|
||||
matrix.r3c1 = corrector2 * (x1x3 - s0x2);
|
||||
|
||||
matrix.r2c1 = corrector2 * (x1x2 + s0x3);
|
||||
matrix.r3c2 = corrector2 * (x2x3 + s0x1);
|
||||
matrix.r1c3 = corrector2 * (x1x3 + s0x2);
|
||||
quaternion.s0 = 1.0f;
|
||||
quaternion.x1 = 0.0f;
|
||||
quaternion.x2 = 0.0f;
|
||||
quaternion.x3 = 0.0f;
|
||||
}
|
||||
|
||||
public readonly void MakeReverseMatrix(out Matrix3x3FP32 matrix)
|
||||
public static void Swap(ref QuaternionFP32 quaternion1, ref QuaternionFP32 quaternion2)
|
||||
{
|
||||
float s0s0 = this.s0 * this.s0;
|
||||
float x1x1 = this.x1 * this.x1;
|
||||
float x2x2 = this.x2 * this.x2;
|
||||
float x3x3 = this.x3 * this.x3;
|
||||
float s0 = quaternion1.s0;
|
||||
float x1 = quaternion1.x1;
|
||||
float x2 = quaternion1.x2;
|
||||
float x3 = quaternion1.x3;
|
||||
|
||||
float squareModulus = (s0s0 + x1x1) + (x2x2 + x3x3);
|
||||
quaternion1.s0 = quaternion2.s0;
|
||||
quaternion1.x1 = quaternion2.x1;
|
||||
quaternion1.x2 = quaternion2.x2;
|
||||
quaternion1.x3 = quaternion2.x3;
|
||||
|
||||
if (squareModulus <= UtilityFP32.SQUARE_EPSYLON)
|
||||
{
|
||||
Matrix3x3FP32.LoadIdentity(out matrix);
|
||||
return;
|
||||
}
|
||||
|
||||
float corrector1 = 1.0f / squareModulus;
|
||||
|
||||
float s0x1 = this.s0 * this.x1;
|
||||
float s0x2 = this.s0 * this.x2;
|
||||
float s0x3 = this.s0 * this.x3;
|
||||
float x1x2 = this.x1 * this.x2;
|
||||
float x1x3 = this.x1 * this.x3;
|
||||
float x2x3 = this.x2 * this.x3;
|
||||
|
||||
float corrector2 = 2.0f * corrector1;
|
||||
|
||||
matrix.r1c1 = corrector1 * ((s0s0 + x1x1) - (x2x2 + x3x3));
|
||||
matrix.r2c2 = corrector1 * ((s0s0 + x2x2) - (x1x1 + x3x3));
|
||||
matrix.r3c3 = corrector1 * ((s0s0 + x3x3) - (x1x1 + x2x2));
|
||||
|
||||
matrix.r1c2 = corrector2 * (x1x2 + s0x3);
|
||||
matrix.r2c3 = corrector2 * (x2x3 + s0x1);
|
||||
matrix.r3c1 = corrector2 * (x1x3 + s0x2);
|
||||
|
||||
matrix.r2c1 = corrector2 * (x1x2 - s0x3);
|
||||
matrix.r3c2 = corrector2 * (x2x3 - s0x1);
|
||||
matrix.r1c3 = corrector2 * (x1x3 - s0x2);
|
||||
quaternion2.s0 = s0;
|
||||
quaternion2.x1 = x1;
|
||||
quaternion2.x2 = x2;
|
||||
quaternion2.x3 = x3;
|
||||
}
|
||||
|
||||
public static void Add(in QuaternionFP32 quaternion1, in QuaternionFP32 quaternion2, out QuaternionFP32 sum)
|
||||
|
|
@ -195,6 +189,14 @@ namespace BasicGeometry
|
|||
sum.x3 = quaternion1.x3 + quaternion2.x3;
|
||||
}
|
||||
|
||||
public static void AddScaled(in QuaternionFP32 basic_quaternion, in QuaternionFP32 scalable_quaternion, float scale, out QuaternionFP32 sum)
|
||||
{
|
||||
sum.s0 = basic_quaternion.s0 + scale * scalable_quaternion.s0;
|
||||
sum.x1 = basic_quaternion.x1 + scale * scalable_quaternion.x1;
|
||||
sum.x2 = basic_quaternion.x2 + scale * scalable_quaternion.x2;
|
||||
sum.x3 = basic_quaternion.x3 + scale * scalable_quaternion.x3;
|
||||
}
|
||||
|
||||
public static void Subtract(in QuaternionFP32 minuend, in QuaternionFP32 subtrahend, out QuaternionFP32 difference)
|
||||
{
|
||||
difference.s0 = minuend.s0 - subtrahend.s0;
|
||||
|
|
@ -216,7 +218,272 @@ namespace BasicGeometry
|
|||
product.x3 = x3;
|
||||
}
|
||||
|
||||
public static bool AreClose(QuaternionFP32 quaternion1, QuaternionFP32 quaternion2)
|
||||
public static void Multiply(in QuaternionFP32 quaternion, float multiplier, out QuaternionFP32 product)
|
||||
{
|
||||
product.s0 = quaternion.s0 * multiplier;
|
||||
product.x1 = quaternion.x1 * multiplier;
|
||||
product.x2 = quaternion.x2 * multiplier;
|
||||
product.x3 = quaternion.x3 * multiplier;
|
||||
}
|
||||
|
||||
public static bool Divide(in QuaternionFP32 divident, in QuaternionFP32 divisor, out QuaternionFP32 quotient)
|
||||
{
|
||||
float squareModulus = divisor.GetSquareModulus();
|
||||
|
||||
if (squareModulus <= UtilityFP32.SQUARE_EPSYLON || float.IsNaN(squareModulus))
|
||||
{
|
||||
Reset(out quotient);
|
||||
return false;
|
||||
}
|
||||
|
||||
float s0 = (divident.s0 * divisor.s0 + divident.x1 * divisor.x1) + (divident.x2 * divisor.x2 + divident.x3 * divisor.x3);
|
||||
float x1 = (divident.x1 * divisor.s0 + divident.x3 * divisor.x2) - (divident.s0 * divisor.x1 + divident.x2 * divisor.x3);
|
||||
float x2 = (divident.x2 * divisor.s0 + divident.x1 * divisor.x3) - (divident.s0 * divisor.x2 + divident.x3 * divisor.x1);
|
||||
float x3 = (divident.x3 * divisor.s0 + divident.x2 * divisor.x1) - (divident.s0 * divisor.x3 + divident.x1 * divisor.x2);
|
||||
|
||||
float multiplicand = 1.0f / squareModulus;
|
||||
|
||||
quotient.s0 = s0 * multiplicand;
|
||||
quotient.x1 = x1 * multiplicand;
|
||||
quotient.x2 = x2 * multiplicand;
|
||||
quotient.x3 = x3 * multiplicand;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
public static void Divide(in QuaternionFP32 divident, float divisor, out QuaternionFP32 quotient)
|
||||
{
|
||||
Multiply(divident, 1.0f / divisor, out quotient);
|
||||
}
|
||||
|
||||
public static void GetMeanOfTwo(in QuaternionFP32 vector1, in QuaternionFP32 vector2, out QuaternionFP32 mean)
|
||||
{
|
||||
mean.s0 = (vector1.s0 + vector2.s0) * 0.5f;
|
||||
mean.x1 = (vector1.x1 + vector2.x1) * 0.5f;
|
||||
mean.x2 = (vector1.x2 + vector2.x2) * 0.5f;
|
||||
mean.x3 = (vector1.x3 + vector2.x3) * 0.5f;
|
||||
}
|
||||
|
||||
public static void GetMeanOfThree(in QuaternionFP32 vector1, in QuaternionFP32 vector2, in QuaternionFP32 vector3, out QuaternionFP32 mean)
|
||||
{
|
||||
mean.s0 = (vector1.s0 + vector2.s0 + vector3.s0) * UtilityFP32.ONE_THIRD;
|
||||
mean.x1 = (vector1.x1 + vector2.x1 + vector3.x1) * UtilityFP32.ONE_THIRD;
|
||||
mean.x2 = (vector1.x2 + vector2.x2 + vector3.x2) * UtilityFP32.ONE_THIRD;
|
||||
mean.x3 = (vector1.x3 + vector2.x3 + vector3.x3) * UtilityFP32.ONE_THIRD;
|
||||
}
|
||||
|
||||
public static void Interpolate(in QuaternionFP32 quaternion1, in QuaternionFP32 quaternion2, float phase, out QuaternionFP32 interpolation)
|
||||
{
|
||||
float counterphase = 1.0f - phase;
|
||||
|
||||
interpolation.s0 = quaternion1.s0 * counterphase + quaternion2.s0 * phase;
|
||||
interpolation.x1 = quaternion1.x1 * counterphase + quaternion2.x1 * phase;
|
||||
interpolation.x2 = quaternion1.x2 * counterphase + quaternion2.x2 * phase;
|
||||
interpolation.x3 = quaternion1.x3 * counterphase + quaternion2.x3 * phase;
|
||||
}
|
||||
|
||||
public static void GetConjugate(in QuaternionFP32 quaternion, out QuaternionFP32 conjugate)
|
||||
{
|
||||
conjugate.s0 = quaternion.s0;
|
||||
conjugate.x1 = -quaternion.x1;
|
||||
conjugate.x2 = -quaternion.x2;
|
||||
conjugate.x3 = -quaternion.x3;
|
||||
}
|
||||
|
||||
public static void GetOpposite(in QuaternionFP32 quaternion, out QuaternionFP32 opposit)
|
||||
{
|
||||
opposit.s0 = -quaternion.s0;
|
||||
opposit.x1 = -quaternion.x1;
|
||||
opposit.x2 = -quaternion.x2;
|
||||
opposit.x3 = -quaternion.x3;
|
||||
}
|
||||
|
||||
public static bool GetInverse(in QuaternionFP32 quaternion, out QuaternionFP32 inverse)
|
||||
{
|
||||
float squareModulus = quaternion.GetSquareModulus();
|
||||
|
||||
if (squareModulus <= UtilityFP32.SQUARE_EPSYLON || float.IsNaN(squareModulus))
|
||||
{
|
||||
Reset(out inverse);
|
||||
return false;
|
||||
}
|
||||
|
||||
float multiplicand = 1.0f / squareModulus;
|
||||
|
||||
inverse.s0 = quaternion.s0 * multiplicand;
|
||||
inverse.x1 = -quaternion.x1 * multiplicand;
|
||||
inverse.x2 = -quaternion.x2 * multiplicand;
|
||||
inverse.x3 = -quaternion.x3 * multiplicand;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
public static bool GetNormalized(in QuaternionFP32 quaternion, out QuaternionFP32 normalized)
|
||||
{
|
||||
float squareModulus = quaternion.GetSquareModulus();
|
||||
|
||||
if (squareModulus <= UtilityFP32.SQUARE_EPSYLON || float.IsNaN(squareModulus))
|
||||
{
|
||||
Reset(out normalized);
|
||||
return false;
|
||||
}
|
||||
|
||||
float multiplier = MathF.Sqrt(1.0f / squareModulus);
|
||||
|
||||
normalized.s0 = quaternion.s0 * multiplier;
|
||||
normalized.x1 = quaternion.x1 * multiplier;
|
||||
normalized.x2 = quaternion.x2 * multiplier;
|
||||
normalized.x3 = quaternion.x3 * multiplier;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
public static bool GetExponation(in QuaternionFP32 quaternion, float exponent, out QuaternionFP32 power)
|
||||
{
|
||||
float s0s0 = quaternion.s0 * quaternion.s0;
|
||||
float x1x1 = quaternion.x1 * quaternion.x1;
|
||||
float x2x2 = quaternion.x2 * quaternion.x2;
|
||||
float x3x3 = quaternion.x3 * quaternion.x3;
|
||||
|
||||
float squareVector = x1x1 + (x2x2 + x3x3);
|
||||
float squareModulus = (s0s0 + x1x1) + (x2x2 + x3x3);
|
||||
|
||||
if (float.IsNaN(squareModulus))
|
||||
{
|
||||
Reset(out power);
|
||||
return false;
|
||||
}
|
||||
|
||||
if (squareModulus <= UtilityFP32.SQUARE_EPSYLON)
|
||||
{
|
||||
Reset(out power);
|
||||
return true;
|
||||
}
|
||||
|
||||
if (squareVector <= UtilityFP32.SQUARE_EPSYLON)
|
||||
{
|
||||
if (quaternion.s0 < 0.0f)
|
||||
{
|
||||
Reset(out power);
|
||||
return false;
|
||||
}
|
||||
|
||||
power.s0 = MathF.Pow(quaternion.s0, exponent);
|
||||
power.x1 = 0.0f;
|
||||
power.x2 = 0.0f;
|
||||
power.x3 = 0.0f;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
float sine = MathF.Sqrt(squareVector / squareModulus);
|
||||
float power_angle = MathF.Atan2(sine, quaternion.s0 / MathF.Sqrt(squareModulus)) * exponent;
|
||||
float power_modulus = MathF.Pow(squareModulus, 0.5f * exponent);
|
||||
float multiplier = power_modulus * MathF.Sin(power_angle) / sine;
|
||||
|
||||
power.s0 = power_modulus * MathF.Cos(power_angle);
|
||||
power.x1 = quaternion.x1 * multiplier;
|
||||
power.x2 = quaternion.x2 * multiplier;
|
||||
power.x3 = quaternion.x3 * multiplier;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
public static bool GetRotationMatrix(in QuaternionFP32 quaternion, out Matrix3x3FP32 matrix)
|
||||
{
|
||||
float s0s0 = quaternion.s0 * quaternion.s0;
|
||||
float x1x1 = quaternion.x1 * quaternion.x1;
|
||||
float x2x2 = quaternion.x2 * quaternion.x2;
|
||||
float x3x3 = quaternion.x3 * quaternion.x3;
|
||||
|
||||
float squareModulus = s0s0 + x1x1 + (x2x2 + x3x3);
|
||||
|
||||
if (squareModulus <= UtilityFP32.SQUARE_EPSYLON)
|
||||
{
|
||||
Matrix3x3FP32.LoadIdentity(out matrix);
|
||||
return false;
|
||||
}
|
||||
|
||||
float corrector1 = 1.0f / squareModulus;
|
||||
|
||||
float s0x1 = quaternion.s0 * quaternion.x1;
|
||||
float s0x2 = quaternion.s0 * quaternion.x2;
|
||||
float s0x3 = quaternion.s0 * quaternion.x3;
|
||||
float x1x2 = quaternion.x1 * quaternion.x2;
|
||||
float x1x3 = quaternion.x1 * quaternion.x3;
|
||||
float x2x3 = quaternion.x2 * quaternion.x3;
|
||||
|
||||
float corrector2 = 2.0f * corrector1;
|
||||
|
||||
matrix.r1c1 = corrector1 * ((s0s0 + x1x1) - (x2x2 + x3x3));
|
||||
matrix.r2c2 = corrector1 * ((s0s0 + x2x2) - (x1x1 + x3x3));
|
||||
matrix.r3c3 = corrector1 * ((s0s0 + x3x3) - (x1x1 + x2x2));
|
||||
|
||||
matrix.r1c2 = corrector2 * (x1x2 - s0x3);
|
||||
matrix.r2c3 = corrector2 * (x2x3 - s0x1);
|
||||
matrix.r3c1 = corrector2 * (x1x3 - s0x2);
|
||||
|
||||
matrix.r2c1 = corrector2 * (x1x2 + s0x3);
|
||||
matrix.r3c2 = corrector2 * (x2x3 + s0x1);
|
||||
matrix.r1c3 = corrector2 * (x1x3 + s0x2);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
public static bool GetReverseMatrix(in QuaternionFP32 quaternion, out Matrix3x3FP32 matrix)
|
||||
{
|
||||
float s0s0 = quaternion.s0 * quaternion.s0;
|
||||
float x1x1 = quaternion.x1 * quaternion.x1;
|
||||
float x2x2 = quaternion.x2 * quaternion.x2;
|
||||
float x3x3 = quaternion.x3 * quaternion.x3;
|
||||
|
||||
float squareModulus = s0s0 + x1x1 + (x2x2 + x3x3);
|
||||
|
||||
if (squareModulus <= UtilityFP32.SQUARE_EPSYLON)
|
||||
{
|
||||
Matrix3x3FP32.LoadIdentity(out matrix);
|
||||
return false;
|
||||
}
|
||||
|
||||
float corrector1 = 1.0f / squareModulus;
|
||||
|
||||
float s0x1 = quaternion.s0 * quaternion.x1;
|
||||
float s0x2 = quaternion.s0 * quaternion.x2;
|
||||
float s0x3 = quaternion.s0 * quaternion.x3;
|
||||
float x1x2 = quaternion.x1 * quaternion.x2;
|
||||
float x1x3 = quaternion.x1 * quaternion.x3;
|
||||
float x2x3 = quaternion.x2 * quaternion.x3;
|
||||
|
||||
float corrector2 = 2.0f * corrector1;
|
||||
|
||||
matrix.r1c1 = corrector1 * ((s0s0 + x1x1) - (x2x2 + x3x3));
|
||||
matrix.r2c2 = corrector1 * ((s0s0 + x2x2) - (x1x1 + x3x3));
|
||||
matrix.r3c3 = corrector1 * ((s0s0 + x3x3) - (x1x1 + x2x2));
|
||||
|
||||
matrix.r1c2 = corrector2 * (x1x2 + s0x3);
|
||||
matrix.r2c3 = corrector2 * (x2x3 + s0x1);
|
||||
matrix.r3c1 = corrector2 * (x1x3 + s0x2);
|
||||
|
||||
matrix.r2c1 = corrector2 * (x1x2 - s0x3);
|
||||
matrix.r3c2 = corrector2 * (x2x3 - s0x1);
|
||||
matrix.r1c3 = corrector2 * (x1x3 - s0x2);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
public static bool GetBothMatrices(in QuaternionFP32 quaternion, out Matrix3x3FP32 matrix, out Matrix3x3FP32 reverse)
|
||||
{
|
||||
if (GetReverseMatrix(quaternion, out reverse))
|
||||
{
|
||||
Matrix3x3FP32.MakeTransposed(reverse, out matrix);
|
||||
return true;
|
||||
}
|
||||
|
||||
Matrix3x3FP32.LoadIdentity(out matrix);
|
||||
return false;
|
||||
}
|
||||
|
||||
public static bool AreClose(in QuaternionFP32 quaternion1, in QuaternionFP32 quaternion2)
|
||||
{
|
||||
float ds0 = quaternion1.s0 - quaternion2.s0;
|
||||
float dx1 = quaternion1.x1 - quaternion2.x1;
|
||||
|
|
|
|||
|
|
@ -4,7 +4,7 @@
|
|||
* Date: 4 Dec 2024
|
||||
*/
|
||||
|
||||
namespace BasicGeometry
|
||||
namespace BGC
|
||||
{
|
||||
public struct QuaternionFP64
|
||||
{
|
||||
|
|
@ -18,7 +18,7 @@ namespace BasicGeometry
|
|||
this.x3 = x3;
|
||||
}
|
||||
|
||||
public QuaternionFP64(in QuaternionFP32 quaternion)
|
||||
public QuaternionFP64(in QuaternionFP64 quaternion)
|
||||
{
|
||||
this.s0 = quaternion.s0;
|
||||
this.x1 = quaternion.x1;
|
||||
|
|
@ -26,7 +26,7 @@ namespace BasicGeometry
|
|||
this.x3 = quaternion.x3;
|
||||
}
|
||||
|
||||
public QuaternionFP64(in QuaternionFP64 quaternion)
|
||||
public QuaternionFP64(in QuaternionFP32 quaternion)
|
||||
{
|
||||
this.s0 = quaternion.s0;
|
||||
this.x1 = quaternion.x1;
|
||||
|
|
@ -62,7 +62,7 @@ namespace BasicGeometry
|
|||
this.x3 = 0.0;
|
||||
}
|
||||
|
||||
public void SetToIdentity()
|
||||
public void MakeUnit()
|
||||
{
|
||||
this.s0 = 1.0;
|
||||
this.x1 = 0.0;
|
||||
|
|
@ -77,6 +77,52 @@ namespace BasicGeometry
|
|||
this.x3 = -this.x3;
|
||||
}
|
||||
|
||||
public void MakeOpposite()
|
||||
{
|
||||
this.s0 = -this.s0;
|
||||
this.x1 = -this.x1;
|
||||
this.x2 = -this.x2;
|
||||
this.x3 = -this.x3;
|
||||
}
|
||||
|
||||
public bool Invert()
|
||||
{
|
||||
double squareModulus = this.GetSquareModulus();
|
||||
|
||||
if (squareModulus <= UtilityFP64.SQUARE_EPSYLON || double.IsNaN(squareModulus))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
double multiplicand = 1.0 / squareModulus;
|
||||
|
||||
this.s0 = this.s0 * multiplicand;
|
||||
this.x1 = -this.x1 * multiplicand;
|
||||
this.x2 = -this.x2 * multiplicand;
|
||||
this.x3 = -this.x3 * multiplicand;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
public bool Normalize()
|
||||
{
|
||||
double squareModulus = this.GetSquareModulus();
|
||||
|
||||
if (squareModulus <= UtilityFP64.SQUARE_EPSYLON || double.IsNaN(squareModulus))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
double multiplier = Math.Sqrt(1.0 / squareModulus);
|
||||
|
||||
this.s0 *= multiplier;
|
||||
this.x1 *= multiplier;
|
||||
this.x2 *= multiplier;
|
||||
this.x3 *= multiplier;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
public void SetValues(double s0, double x1, double x2, double x3)
|
||||
{
|
||||
this.s0 = s0;
|
||||
|
|
@ -85,7 +131,7 @@ namespace BasicGeometry
|
|||
this.x3 = x3;
|
||||
}
|
||||
|
||||
public void Set(in QuaternionFP32 quaternion)
|
||||
public void SetValues(in QuaternionFP64 quaternion)
|
||||
{
|
||||
this.s0 = quaternion.s0;
|
||||
this.x1 = quaternion.x1;
|
||||
|
|
@ -93,7 +139,7 @@ namespace BasicGeometry
|
|||
this.x3 = quaternion.x3;
|
||||
}
|
||||
|
||||
public void Set(in QuaternionFP64 quaternion)
|
||||
public void SetValues(in QuaternionFP32 quaternion)
|
||||
{
|
||||
this.s0 = quaternion.s0;
|
||||
this.x1 = quaternion.x1;
|
||||
|
|
@ -101,90 +147,38 @@ namespace BasicGeometry
|
|||
this.x3 = quaternion.x3;
|
||||
}
|
||||
|
||||
public static void MakeConjugate(in QuaternionFP64 quaternion, out QuaternionFP64 conjugate)
|
||||
public static void Reset(out QuaternionFP64 quaternion)
|
||||
{
|
||||
conjugate.s0 = quaternion.s0;
|
||||
conjugate.x1 = -quaternion.x1;
|
||||
conjugate.x2 = -quaternion.x2;
|
||||
conjugate.x3 = -quaternion.x3;
|
||||
quaternion.s0 = 0.0;
|
||||
quaternion.x1 = 0.0;
|
||||
quaternion.x2 = 0.0;
|
||||
quaternion.x3 = 0.0;
|
||||
}
|
||||
|
||||
public readonly void MakeRotationMatrix(out Matrix3x3FP64 matrix)
|
||||
public static void MakeUnit(out QuaternionFP64 quaternion)
|
||||
{
|
||||
double s0s0 = this.s0 * this.s0;
|
||||
double x1x1 = this.x1 * this.x1;
|
||||
double x2x2 = this.x2 * this.x2;
|
||||
double x3x3 = this.x3 * this.x3;
|
||||
|
||||
double squareModulus = (s0s0 + x1x1) + (x2x2 + x3x3);
|
||||
|
||||
if (squareModulus <= UtilityFP64.SQUARE_EPSYLON || !double.IsFinite(squareModulus))
|
||||
{
|
||||
Matrix3x3FP64.LoadIdentity(out matrix);
|
||||
return;
|
||||
}
|
||||
|
||||
double corrector1 = 1.0 / squareModulus;
|
||||
|
||||
double s0x1 = this.s0 * this.x1;
|
||||
double s0x2 = this.s0 * this.x2;
|
||||
double s0x3 = this.s0 * this.x3;
|
||||
double x1x2 = this.x1 * this.x2;
|
||||
double x1x3 = this.x1 * this.x3;
|
||||
double x2x3 = this.x2 * this.x3;
|
||||
|
||||
double corrector2 = 2.0 * corrector1;
|
||||
|
||||
matrix.r1c1 = corrector1 * ((s0s0 + x1x1) - (x2x2 + x3x3));
|
||||
matrix.r2c2 = corrector1 * ((s0s0 + x2x2) - (x1x1 + x3x3));
|
||||
matrix.r3c3 = corrector1 * ((s0s0 + x3x3) - (x1x1 + x2x2));
|
||||
|
||||
matrix.r1c2 = corrector2 * (x1x2 - s0x3);
|
||||
matrix.r2c3 = corrector2 * (x2x3 - s0x1);
|
||||
matrix.r3c1 = corrector2 * (x1x3 - s0x2);
|
||||
|
||||
matrix.r2c1 = corrector2 * (x1x2 + s0x3);
|
||||
matrix.r3c2 = corrector2 * (x2x3 + s0x1);
|
||||
matrix.r1c3 = corrector2 * (x1x3 + s0x2);
|
||||
quaternion.s0 = 1.0;
|
||||
quaternion.x1 = 0.0;
|
||||
quaternion.x2 = 0.0;
|
||||
quaternion.x3 = 0.0;
|
||||
}
|
||||
|
||||
public readonly void MakeReverseMatrix(out Matrix3x3FP64 matrix)
|
||||
public static void Swap(ref QuaternionFP64 quaternion1, ref QuaternionFP64 quaternion2)
|
||||
{
|
||||
double s0s0 = this.s0 * this.s0;
|
||||
double x1x1 = this.x1 * this.x1;
|
||||
double x2x2 = this.x2 * this.x2;
|
||||
double x3x3 = this.x3 * this.x3;
|
||||
double s0 = quaternion1.s0;
|
||||
double x1 = quaternion1.x1;
|
||||
double x2 = quaternion1.x2;
|
||||
double x3 = quaternion1.x3;
|
||||
|
||||
double squareModulus = (s0s0 + x1x1) + (x2x2 + x3x3);
|
||||
quaternion1.s0 = quaternion2.s0;
|
||||
quaternion1.x1 = quaternion2.x1;
|
||||
quaternion1.x2 = quaternion2.x2;
|
||||
quaternion1.x3 = quaternion2.x3;
|
||||
|
||||
if (squareModulus <= UtilityFP64.SQUARE_EPSYLON || !double.IsFinite(squareModulus))
|
||||
{
|
||||
Matrix3x3FP64.LoadIdentity(out matrix);
|
||||
return;
|
||||
}
|
||||
|
||||
double corrector1 = 1.0 / squareModulus;
|
||||
|
||||
double s0x1 = this.s0 * this.x1;
|
||||
double s0x2 = this.s0 * this.x2;
|
||||
double s0x3 = this.s0 * this.x3;
|
||||
double x1x2 = this.x1 * this.x2;
|
||||
double x1x3 = this.x1 * this.x3;
|
||||
double x2x3 = this.x2 * this.x3;
|
||||
|
||||
double corrector2 = 2.0 * corrector1;
|
||||
|
||||
matrix.r1c1 = corrector1 * ((s0s0 + x1x1) - (x2x2 + x3x3));
|
||||
matrix.r2c2 = corrector1 * ((s0s0 + x2x2) - (x1x1 + x3x3));
|
||||
matrix.r3c3 = corrector1 * ((s0s0 + x3x3) - (x1x1 + x2x2));
|
||||
|
||||
matrix.r1c2 = corrector2 * (x1x2 + s0x3);
|
||||
matrix.r2c3 = corrector2 * (x2x3 + s0x1);
|
||||
matrix.r3c1 = corrector2 * (x1x3 + s0x2);
|
||||
|
||||
matrix.r2c1 = corrector2 * (x1x2 - s0x3);
|
||||
matrix.r3c2 = corrector2 * (x2x3 - s0x1);
|
||||
matrix.r1c3 = corrector2 * (x1x3 - s0x2);
|
||||
quaternion2.s0 = s0;
|
||||
quaternion2.x1 = x1;
|
||||
quaternion2.x2 = x2;
|
||||
quaternion2.x3 = x3;
|
||||
}
|
||||
|
||||
public static void Add(in QuaternionFP64 quaternion1, in QuaternionFP64 quaternion2, out QuaternionFP64 sum)
|
||||
|
|
@ -195,6 +189,14 @@ namespace BasicGeometry
|
|||
sum.x3 = quaternion1.x3 + quaternion2.x3;
|
||||
}
|
||||
|
||||
public static void AddScaled(in QuaternionFP64 basic_quaternion, in QuaternionFP64 scalable_quaternion, double scale, out QuaternionFP64 sum)
|
||||
{
|
||||
sum.s0 = basic_quaternion.s0 + scale * scalable_quaternion.s0;
|
||||
sum.x1 = basic_quaternion.x1 + scale * scalable_quaternion.x1;
|
||||
sum.x2 = basic_quaternion.x2 + scale * scalable_quaternion.x2;
|
||||
sum.x3 = basic_quaternion.x3 + scale * scalable_quaternion.x3;
|
||||
}
|
||||
|
||||
public static void Subtract(in QuaternionFP64 minuend, in QuaternionFP64 subtrahend, out QuaternionFP64 difference)
|
||||
{
|
||||
difference.s0 = minuend.s0 - subtrahend.s0;
|
||||
|
|
@ -216,6 +218,271 @@ namespace BasicGeometry
|
|||
product.x3 = x3;
|
||||
}
|
||||
|
||||
public static void Multiply(in QuaternionFP64 quaternion, double multiplier, out QuaternionFP64 product)
|
||||
{
|
||||
product.s0 = quaternion.s0 * multiplier;
|
||||
product.x1 = quaternion.x1 * multiplier;
|
||||
product.x2 = quaternion.x2 * multiplier;
|
||||
product.x3 = quaternion.x3 * multiplier;
|
||||
}
|
||||
|
||||
public static bool Divide(in QuaternionFP64 divident, in QuaternionFP64 divisor, out QuaternionFP64 quotient)
|
||||
{
|
||||
double squareModulus = divisor.GetSquareModulus();
|
||||
|
||||
if (squareModulus <= UtilityFP64.SQUARE_EPSYLON || double.IsNaN(squareModulus))
|
||||
{
|
||||
Reset(out quotient);
|
||||
return false;
|
||||
}
|
||||
|
||||
double s0 = (divident.s0 * divisor.s0 + divident.x1 * divisor.x1) + (divident.x2 * divisor.x2 + divident.x3 * divisor.x3);
|
||||
double x1 = (divident.x1 * divisor.s0 + divident.x3 * divisor.x2) - (divident.s0 * divisor.x1 + divident.x2 * divisor.x3);
|
||||
double x2 = (divident.x2 * divisor.s0 + divident.x1 * divisor.x3) - (divident.s0 * divisor.x2 + divident.x3 * divisor.x1);
|
||||
double x3 = (divident.x3 * divisor.s0 + divident.x2 * divisor.x1) - (divident.s0 * divisor.x3 + divident.x1 * divisor.x2);
|
||||
|
||||
double multiplicand = 1.0 / squareModulus;
|
||||
|
||||
quotient.s0 = s0 * multiplicand;
|
||||
quotient.x1 = x1 * multiplicand;
|
||||
quotient.x2 = x2 * multiplicand;
|
||||
quotient.x3 = x3 * multiplicand;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
public static void Divide(in QuaternionFP64 divident, double divisor, out QuaternionFP64 quotient)
|
||||
{
|
||||
Multiply(divident, 1.0 / divisor, out quotient);
|
||||
}
|
||||
|
||||
public static void GetMeanOfTwo(in QuaternionFP64 vector1, in QuaternionFP64 vector2, out QuaternionFP64 mean)
|
||||
{
|
||||
mean.s0 = (vector1.s0 + vector2.s0) * 0.5;
|
||||
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 QuaternionFP64 vector1, in QuaternionFP64 vector2, in QuaternionFP64 vector3, out QuaternionFP64 mean)
|
||||
{
|
||||
mean.s0 = (vector1.s0 + vector2.s0 + vector3.s0) * UtilityFP64.ONE_THIRD;
|
||||
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 QuaternionFP64 quaternion1, in QuaternionFP64 quaternion2, double phase, out QuaternionFP64 interpolation)
|
||||
{
|
||||
double counterphase = 1.0 - phase;
|
||||
|
||||
interpolation.s0 = quaternion1.s0 * counterphase + quaternion2.s0 * phase;
|
||||
interpolation.x1 = quaternion1.x1 * counterphase + quaternion2.x1 * phase;
|
||||
interpolation.x2 = quaternion1.x2 * counterphase + quaternion2.x2 * phase;
|
||||
interpolation.x3 = quaternion1.x3 * counterphase + quaternion2.x3 * phase;
|
||||
}
|
||||
|
||||
public static void GetConjugate(in QuaternionFP64 quaternion, out QuaternionFP64 conjugate)
|
||||
{
|
||||
conjugate.s0 = quaternion.s0;
|
||||
conjugate.x1 = -quaternion.x1;
|
||||
conjugate.x2 = -quaternion.x2;
|
||||
conjugate.x3 = -quaternion.x3;
|
||||
}
|
||||
|
||||
public static void GetOpposite(in QuaternionFP64 quaternion, out QuaternionFP64 opposit)
|
||||
{
|
||||
opposit.s0 = -quaternion.s0;
|
||||
opposit.x1 = -quaternion.x1;
|
||||
opposit.x2 = -quaternion.x2;
|
||||
opposit.x3 = -quaternion.x3;
|
||||
}
|
||||
|
||||
public static bool GetInverse(in QuaternionFP64 quaternion, out QuaternionFP64 inverse)
|
||||
{
|
||||
double squareModulus = quaternion.GetSquareModulus();
|
||||
|
||||
if (squareModulus <= UtilityFP64.SQUARE_EPSYLON || double.IsNaN(squareModulus))
|
||||
{
|
||||
Reset(out inverse);
|
||||
return false;
|
||||
}
|
||||
|
||||
double multiplicand = 1.0 / squareModulus;
|
||||
|
||||
inverse.s0 = quaternion.s0 * multiplicand;
|
||||
inverse.x1 = -quaternion.x1 * multiplicand;
|
||||
inverse.x2 = -quaternion.x2 * multiplicand;
|
||||
inverse.x3 = -quaternion.x3 * multiplicand;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
public static bool GetNormalized(in QuaternionFP64 quaternion, out QuaternionFP64 normalized)
|
||||
{
|
||||
double squareModulus = quaternion.GetSquareModulus();
|
||||
|
||||
if (squareModulus <= UtilityFP64.SQUARE_EPSYLON || double.IsNaN(squareModulus))
|
||||
{
|
||||
Reset(out normalized);
|
||||
return false;
|
||||
}
|
||||
|
||||
double multiplier = Math.Sqrt(1.0 / squareModulus);
|
||||
|
||||
normalized.s0 = quaternion.s0 * multiplier;
|
||||
normalized.x1 = quaternion.x1 * multiplier;
|
||||
normalized.x2 = quaternion.x2 * multiplier;
|
||||
normalized.x3 = quaternion.x3 * multiplier;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
public static bool GetExponation(in QuaternionFP64 quaternion, double exponent, out QuaternionFP64 power)
|
||||
{
|
||||
double s0s0 = quaternion.s0 * quaternion.s0;
|
||||
double x1x1 = quaternion.x1 * quaternion.x1;
|
||||
double x2x2 = quaternion.x2 * quaternion.x2;
|
||||
double x3x3 = quaternion.x3 * quaternion.x3;
|
||||
|
||||
double squareVector = x1x1 + (x2x2 + x3x3);
|
||||
double squareModulus = (s0s0 + x1x1) + (x2x2 + x3x3);
|
||||
|
||||
if (double.IsNaN(squareModulus))
|
||||
{
|
||||
Reset(out power);
|
||||
return false;
|
||||
}
|
||||
|
||||
if (squareModulus <= UtilityFP64.SQUARE_EPSYLON)
|
||||
{
|
||||
Reset(out power);
|
||||
return true;
|
||||
}
|
||||
|
||||
if (squareVector <= UtilityFP64.SQUARE_EPSYLON)
|
||||
{
|
||||
if (quaternion.s0 < 0.0f)
|
||||
{
|
||||
Reset(out power);
|
||||
return false;
|
||||
}
|
||||
|
||||
power.s0 = Math.Pow(quaternion.s0, exponent);
|
||||
power.x1 = 0.0f;
|
||||
power.x2 = 0.0f;
|
||||
power.x3 = 0.0f;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
double sine = Math.Sqrt(squareVector / squareModulus);
|
||||
double power_angle = Math.Atan2(sine, quaternion.s0 / Math.Sqrt(squareModulus)) * exponent;
|
||||
double power_modulus = Math.Pow(squareModulus, 0.5f * exponent);
|
||||
double multiplier = power_modulus * Math.Sin(power_angle) / sine;
|
||||
|
||||
power.s0 = power_modulus * Math.Cos(power_angle);
|
||||
power.x1 = quaternion.x1 * multiplier;
|
||||
power.x2 = quaternion.x2 * multiplier;
|
||||
power.x3 = quaternion.x3 * multiplier;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
public static bool GetRotationMatrix(in QuaternionFP64 quaternion, out Matrix3x3FP64 matrix)
|
||||
{
|
||||
double s0s0 = quaternion.s0 * quaternion.s0;
|
||||
double x1x1 = quaternion.x1 * quaternion.x1;
|
||||
double x2x2 = quaternion.x2 * quaternion.x2;
|
||||
double x3x3 = quaternion.x3 * quaternion.x3;
|
||||
|
||||
double squareModulus = (s0s0 + x1x1) + (x2x2 + x3x3);
|
||||
|
||||
if (squareModulus <= UtilityFP64.SQUARE_EPSYLON || !double.IsFinite(squareModulus))
|
||||
{
|
||||
Matrix3x3FP64.LoadIdentity(out matrix);
|
||||
return false;
|
||||
}
|
||||
|
||||
double corrector1 = 1.0 / squareModulus;
|
||||
|
||||
double s0x1 = quaternion.s0 * quaternion.x1;
|
||||
double s0x2 = quaternion.s0 * quaternion.x2;
|
||||
double s0x3 = quaternion.s0 * quaternion.x3;
|
||||
double x1x2 = quaternion.x1 * quaternion.x2;
|
||||
double x1x3 = quaternion.x1 * quaternion.x3;
|
||||
double x2x3 = quaternion.x2 * quaternion.x3;
|
||||
|
||||
double corrector2 = 2.0 * corrector1;
|
||||
|
||||
matrix.r1c1 = corrector1 * ((s0s0 + x1x1) - (x2x2 + x3x3));
|
||||
matrix.r2c2 = corrector1 * ((s0s0 + x2x2) - (x1x1 + x3x3));
|
||||
matrix.r3c3 = corrector1 * ((s0s0 + x3x3) - (x1x1 + x2x2));
|
||||
|
||||
matrix.r1c2 = corrector2 * (x1x2 - s0x3);
|
||||
matrix.r2c3 = corrector2 * (x2x3 - s0x1);
|
||||
matrix.r3c1 = corrector2 * (x1x3 - s0x2);
|
||||
|
||||
matrix.r2c1 = corrector2 * (x1x2 + s0x3);
|
||||
matrix.r3c2 = corrector2 * (x2x3 + s0x1);
|
||||
matrix.r1c3 = corrector2 * (x1x3 + s0x2);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
public static bool GetReverseMatrix(in QuaternionFP64 quaternion, out Matrix3x3FP64 matrix)
|
||||
{
|
||||
double s0s0 = quaternion.s0 * quaternion.s0;
|
||||
double x1x1 = quaternion.x1 * quaternion.x1;
|
||||
double x2x2 = quaternion.x2 * quaternion.x2;
|
||||
double x3x3 = quaternion.x3 * quaternion.x3;
|
||||
|
||||
double squareModulus = (s0s0 + x1x1) + (x2x2 + x3x3);
|
||||
|
||||
if (squareModulus <= UtilityFP64.SQUARE_EPSYLON || !double.IsFinite(squareModulus))
|
||||
{
|
||||
Matrix3x3FP64.LoadIdentity(out matrix);
|
||||
return false;
|
||||
}
|
||||
|
||||
double corrector1 = 1.0 / squareModulus;
|
||||
|
||||
double s0x1 = quaternion.s0 * quaternion.x1;
|
||||
double s0x2 = quaternion.s0 * quaternion.x2;
|
||||
double s0x3 = quaternion.s0 * quaternion.x3;
|
||||
double x1x2 = quaternion.x1 * quaternion.x2;
|
||||
double x1x3 = quaternion.x1 * quaternion.x3;
|
||||
double x2x3 = quaternion.x2 * quaternion.x3;
|
||||
|
||||
double corrector2 = 2.0 * corrector1;
|
||||
|
||||
matrix.r1c1 = corrector1 * ((s0s0 + x1x1) - (x2x2 + x3x3));
|
||||
matrix.r2c2 = corrector1 * ((s0s0 + x2x2) - (x1x1 + x3x3));
|
||||
matrix.r3c3 = corrector1 * ((s0s0 + x3x3) - (x1x1 + x2x2));
|
||||
|
||||
matrix.r1c2 = corrector2 * (x1x2 + s0x3);
|
||||
matrix.r2c3 = corrector2 * (x2x3 + s0x1);
|
||||
matrix.r3c1 = corrector2 * (x1x3 + s0x2);
|
||||
|
||||
matrix.r2c1 = corrector2 * (x1x2 - s0x3);
|
||||
matrix.r3c2 = corrector2 * (x2x3 - s0x1);
|
||||
matrix.r1c3 = corrector2 * (x1x3 - s0x2);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
public static bool GetBothMatrices(in QuaternionFP64 quaternion, out Matrix3x3FP64 matrix, out Matrix3x3FP64 reverse)
|
||||
{
|
||||
if (GetReverseMatrix(quaternion, out reverse))
|
||||
{
|
||||
Matrix3x3FP64.GetTransposed(reverse, out matrix);
|
||||
return true;
|
||||
}
|
||||
|
||||
Matrix3x3FP64.LoadIdentity(out matrix);
|
||||
return false;
|
||||
}
|
||||
|
||||
public static bool AreClose(QuaternionFP32 quaternion1, QuaternionFP32 quaternion2)
|
||||
{
|
||||
double ds0 = quaternion1.s0 - quaternion2.s0;
|
||||
|
|
|
|||
|
|
@ -4,7 +4,7 @@
|
|||
* Date: 18 Nov 2024
|
||||
*/
|
||||
|
||||
namespace BasicGeometry
|
||||
namespace BGC
|
||||
{
|
||||
public class RadianFP32
|
||||
{
|
||||
|
|
|
|||
|
|
@ -4,7 +4,7 @@
|
|||
* Date: 18 Nov 2024
|
||||
*/
|
||||
|
||||
namespace BasicGeometry
|
||||
namespace BGC
|
||||
{
|
||||
public class RadianFP64
|
||||
{
|
||||
|
|
|
|||
|
|
@ -4,7 +4,7 @@
|
|||
* Date: 2 Feb 2019
|
||||
*/
|
||||
|
||||
namespace BasicGeometry
|
||||
namespace BGC
|
||||
{
|
||||
public struct Rotation3FP32
|
||||
{
|
||||
|
|
|
|||
|
|
@ -4,7 +4,7 @@
|
|||
* Date: 2 Feb 2019
|
||||
*/
|
||||
|
||||
namespace BasicGeometry
|
||||
namespace BGC
|
||||
{
|
||||
public struct Rotation3FP64
|
||||
{
|
||||
|
|
|
|||
|
|
@ -4,7 +4,7 @@
|
|||
* Date: 18 Nov 2024
|
||||
*/
|
||||
|
||||
namespace BasicGeometry
|
||||
namespace BGC
|
||||
{
|
||||
public class TurnFP32
|
||||
{
|
||||
|
|
|
|||
|
|
@ -4,7 +4,7 @@
|
|||
* Date: 18 Nov 2024
|
||||
*/
|
||||
|
||||
namespace BasicGeometry
|
||||
namespace BGC
|
||||
{
|
||||
public class TurnFP64
|
||||
{
|
||||
|
|
|
|||
|
|
@ -4,12 +4,12 @@
|
|||
* Date: 12 Nov 2024
|
||||
*/
|
||||
|
||||
namespace BasicGeometry
|
||||
namespace BGC
|
||||
{
|
||||
public class UtilityFP32
|
||||
{
|
||||
public const float EPSYLON = 4.76837E-7f;
|
||||
public const float SQUARE_EPSYLON = 2.27373906E-13f;
|
||||
public const float SQUARE_EPSYLON = EPSYLON * EPSYLON;
|
||||
|
||||
public const float EPSYLON_EFFECTIVENESS_LIMIT = 1.0f;
|
||||
|
||||
|
|
|
|||
|
|
@ -4,12 +4,12 @@
|
|||
* Date: 12 Nov 2024
|
||||
*/
|
||||
|
||||
namespace BasicGeometry
|
||||
namespace BGC
|
||||
{
|
||||
public class UtilityFP64
|
||||
{
|
||||
public const double EPSYLON = 4.996003611E-14;
|
||||
public const double SQUARE_EPSYLON = 2.496005208112504E-27;
|
||||
public const double SQUARE_EPSYLON = EPSYLON * EPSYLON;
|
||||
|
||||
public const double EPSYLON_EFFECTIVENESS_LIMIT = 1.0;
|
||||
|
||||
|
|
|
|||
|
|
@ -4,7 +4,7 @@
|
|||
* Date: 1 Feb 2019
|
||||
*/
|
||||
|
||||
namespace BasicGeometry
|
||||
namespace BGC
|
||||
{
|
||||
public struct Vector2FP32
|
||||
{
|
||||
|
|
@ -41,35 +41,6 @@ namespace BasicGeometry
|
|||
return MathF.Sqrt(this.GetSquareModulus());
|
||||
}
|
||||
|
||||
public int Normalize()
|
||||
{
|
||||
float squareModulus = this.GetSquareModulus();
|
||||
|
||||
if (UtilityFP32.IsSqareUnit(squareModulus))
|
||||
{
|
||||
return 1;
|
||||
}
|
||||
|
||||
if (squareModulus <= UtilityFP32.SQUARE_EPSYLON)
|
||||
{
|
||||
this.Reset();
|
||||
return 0;
|
||||
}
|
||||
|
||||
float multiplier = MathF.Sqrt(1.0f / squareModulus);
|
||||
|
||||
this.x1 *= multiplier;
|
||||
this.x2 *= multiplier;
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
public void Reverse()
|
||||
{
|
||||
this.x1 = -this.x1;
|
||||
this.x2 = -this.x2;
|
||||
}
|
||||
|
||||
public readonly bool IsZero()
|
||||
{
|
||||
return this.GetSquareModulus() <= UtilityFP32.SQUARE_EPSYLON;
|
||||
|
|
@ -86,6 +57,39 @@ namespace BasicGeometry
|
|||
this.x2 = 0.0f;
|
||||
}
|
||||
|
||||
public void MakeOpposite()
|
||||
{
|
||||
this.x1 = -this.x1;
|
||||
this.x2 = -this.x2;
|
||||
}
|
||||
|
||||
public readonly Vector2FP32 GetOpposite()
|
||||
{
|
||||
return new Vector2FP32(-this.x1, -this.x2);
|
||||
}
|
||||
|
||||
public bool Normalize()
|
||||
{
|
||||
float squareModulus = this.GetSquareModulus();
|
||||
|
||||
if (UtilityFP32.IsSqareUnit(squareModulus))
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
if (squareModulus <= UtilityFP32.SQUARE_EPSYLON || float.IsNaN(squareModulus))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
float multiplier = MathF.Sqrt(1.0f / squareModulus);
|
||||
|
||||
this.x1 *= multiplier;
|
||||
this.x2 *= multiplier;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
public void SetValues(float x1, float x2)
|
||||
{
|
||||
this.x1 = x1;
|
||||
|
|
@ -103,28 +107,59 @@ namespace BasicGeometry
|
|||
this.x1 = (float)vector.x1;
|
||||
this.x2 = (float)vector.x2;
|
||||
}
|
||||
|
||||
public void SetReverseOf(in Vector2FP32 vector)
|
||||
{
|
||||
this.x1 = -vector.x1;
|
||||
this.x2 = -vector.x2;
|
||||
}
|
||||
|
||||
public void SetReverseOf(in Vector2FP64 vector)
|
||||
{
|
||||
this.x1 = -(float)vector.x1;
|
||||
this.x2 = -(float)vector.x2;
|
||||
}
|
||||
|
||||
public void AppendScaled(Vector2FP32 summand, float scale)
|
||||
{
|
||||
this.x1 += summand.x1 * scale;
|
||||
this.x2 += summand.x2 * scale;
|
||||
}
|
||||
|
||||
public readonly override string ToString()
|
||||
{
|
||||
return String.Format("SPVector2({0}, {1})", this.x1, this.x2);
|
||||
return String.Format("Vector2FP32({0}, {1})", this.x1, this.x2);
|
||||
}
|
||||
|
||||
public static void Swap(ref Vector2FP32 vector1, ref Vector2FP32 vector2)
|
||||
{
|
||||
float x1 = vector1.x1;
|
||||
float x2 = vector1.x2;
|
||||
|
||||
vector1.x1 = vector2.x1;
|
||||
vector1.x2 = vector2.x2;
|
||||
|
||||
vector2.x1 = x1;
|
||||
vector2.x2 = x2;
|
||||
}
|
||||
|
||||
public static void Reset(out Vector2FP32 vector)
|
||||
{
|
||||
vector.x1 = 0.0f;
|
||||
vector.x2 = 0.0f;
|
||||
}
|
||||
|
||||
public static void GetOpposite(in Vector2FP32 vector, out Vector2FP32 reverted)
|
||||
{
|
||||
reverted.x1 = -vector.x1;
|
||||
reverted.x2 = -vector.x2;
|
||||
}
|
||||
|
||||
public static bool GetNormalized(in Vector2FP32 vector, out Vector2FP32 normalized)
|
||||
{
|
||||
float squareModulus = vector.GetSquareModulus();
|
||||
|
||||
if (UtilityFP32.IsSqareUnit(squareModulus))
|
||||
{
|
||||
normalized.x1 = vector.x1;
|
||||
normalized.x2 = vector.x2;
|
||||
return true;
|
||||
}
|
||||
|
||||
if (squareModulus <= UtilityFP32.SQUARE_EPSYLON || float.IsNaN(squareModulus))
|
||||
{
|
||||
normalized.x1 = 0.0f;
|
||||
normalized.x2 = 0.0f;
|
||||
return false;
|
||||
}
|
||||
|
||||
float multiplier = MathF.Sqrt(1.0f / squareModulus);
|
||||
|
||||
normalized.x1 = vector.x1 * multiplier;
|
||||
normalized.x2 = vector.x2 * multiplier;
|
||||
return true;
|
||||
}
|
||||
|
||||
public static void Add(in Vector2FP32 vector1, in Vector2FP32 vector2, out Vector2FP32 sum)
|
||||
|
|
@ -133,6 +168,12 @@ namespace BasicGeometry
|
|||
sum.x2 = vector1.x2 + vector2.x2;
|
||||
}
|
||||
|
||||
public static void AddScaled(in Vector2FP32 basicVector, in Vector2FP32 scalableVector, float scale, out Vector2FP32 sum)
|
||||
{
|
||||
sum.x1 = basicVector.x1 + scalableVector.x1 * scale;
|
||||
sum.x2 = basicVector.x2 + scalableVector.x2 * scale;
|
||||
}
|
||||
|
||||
public static void Subtract(in Vector2FP32 minuend, in Vector2FP32 subtrahend, out Vector2FP32 difference)
|
||||
{
|
||||
difference.x1 = minuend.x1 - subtrahend.x1;
|
||||
|
|
@ -150,16 +191,24 @@ namespace BasicGeometry
|
|||
Multiply(dividend, 1.0f / divisor, out quotient);
|
||||
}
|
||||
|
||||
public static void GetMean2(in Vector2FP32 vector1, in Vector2FP32 vector2, out Vector2FP32 result)
|
||||
public static void GetMeanOfTwo(in Vector2FP32 vector1, in Vector2FP32 vector2, out Vector2FP32 mean)
|
||||
{
|
||||
result.x1 = (vector1.x1 + vector2.x1) * 0.5f;
|
||||
result.x2 = (vector1.x2 + vector2.x2) * 0.5f;
|
||||
mean.x1 = (vector1.x1 + vector2.x1) * 0.5f;
|
||||
mean.x2 = (vector1.x2 + vector2.x2) * 0.5f;
|
||||
}
|
||||
|
||||
public static void GetMean3(in Vector2FP32 vector1, in Vector2FP32 vector2, in Vector2FP32 vector3, out Vector2FP32 result)
|
||||
public static void GetMeanOfThree(in Vector2FP32 vector1, in Vector2FP32 vector2, in Vector2FP32 vector3, out Vector2FP32 mean)
|
||||
{
|
||||
result.x1 = (vector1.x1 + vector2.x1 + vector3.x1) * UtilityFP32.ONE_THIRD;
|
||||
result.x2 = (vector1.x2 + vector2.x2 + vector3.x2) * UtilityFP32.ONE_THIRD;
|
||||
mean.x1 = (vector1.x1 + vector2.x1 + vector3.x1) * UtilityFP32.ONE_THIRD;
|
||||
mean.x2 = (vector1.x2 + vector2.x2 + vector3.x2) * UtilityFP32.ONE_THIRD;
|
||||
}
|
||||
|
||||
public static void Interpolate(in Vector2FP32 vector1, in Vector2FP32 vector2, float phase, out Vector2FP32 interpolation)
|
||||
{
|
||||
float counterphase = 1.0f - phase;
|
||||
|
||||
interpolation.x1 = vector1.x1 * counterphase + vector2.x1 * phase;
|
||||
interpolation.x2 = vector1.x2 * counterphase + vector2.x2 * phase;
|
||||
}
|
||||
|
||||
public static float GetScalarProduct(in Vector2FP32 vector1, in Vector2FP32 vector2)
|
||||
|
|
@ -176,31 +225,24 @@ namespace BasicGeometry
|
|||
{
|
||||
float squareModulus1 = vector1.GetSquareModulus();
|
||||
|
||||
if (squareModulus1 <= UtilityFP32.SQUARE_EPSYLON)
|
||||
if (squareModulus1 <= UtilityFP32.SQUARE_EPSYLON || float.IsNaN(squareModulus1))
|
||||
{
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
float squareModulus2 = vector2.GetSquareModulus();
|
||||
|
||||
if (squareModulus2 <= UtilityFP32.SQUARE_EPSYLON)
|
||||
if (squareModulus2 <= UtilityFP32.SQUARE_EPSYLON || float.IsNaN(squareModulus2))
|
||||
{
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
float cosine = Vector2FP32.GetScalarProduct(vector1, vector2) / MathF.Sqrt(squareModulus1 * squareModulus2);
|
||||
float multiplier = MathF.Sqrt(1.0f / (squareModulus1 * squareModulus2));
|
||||
|
||||
if (1.0f - UtilityFP32.EPSYLON <= cosine)
|
||||
{
|
||||
return 0.0f;
|
||||
}
|
||||
float x = GetScalarProduct(vector1, vector2);
|
||||
float y = GetCrossProduct(vector1, vector2);
|
||||
|
||||
if (cosine <= -(1.0f - UtilityFP32.EPSYLON))
|
||||
{
|
||||
return AngleFP32.GetHalfCircle(unit);
|
||||
}
|
||||
|
||||
return RadianFP32.ToUnits(MathF.Acos(cosine), unit);
|
||||
return RadianFP32.ToUnits(MathF.Atan2(y * multiplier, x * multiplier), unit);
|
||||
}
|
||||
|
||||
public static float GetSquareDistance(in Vector2FP32 vector1, in Vector2FP32 vector2)
|
||||
|
|
@ -215,25 +257,82 @@ namespace BasicGeometry
|
|||
{
|
||||
return MathF.Sqrt(GetSquareDistance(vector1, vector2));
|
||||
}
|
||||
|
||||
public static bool AreEqual(in Vector2FP32 vector1, in Vector2FP32 vector2)
|
||||
|
||||
public static bool AreCloseEnough(in Vector2FP32 vector1, in Vector2FP32 vector2, float distanceLimit)
|
||||
{
|
||||
return 0.0f <= distanceLimit && GetSquareDistance(vector1, vector2) <= distanceLimit * distanceLimit;
|
||||
}
|
||||
|
||||
public static bool AreClose(in Vector2FP32 vector1, in Vector2FP32 vector2)
|
||||
{
|
||||
float squareModulus1 = vector1.GetSquareModulus();
|
||||
float squareModulus2 = vector2.GetSquareModulus();
|
||||
float squareModulus3 = GetSquareDistance(vector1, vector2);
|
||||
float squareDistance = GetSquareDistance(vector1, vector2);
|
||||
|
||||
// 2.0f means dimension amount
|
||||
if (squareModulus1 < UtilityFP32.EPSYLON_EFFECTIVENESS_LIMIT || squareModulus2 < UtilityFP32.EPSYLON_EFFECTIVENESS_LIMIT)
|
||||
if (squareModulus1 <= UtilityFP32.EPSYLON_EFFECTIVENESS_LIMIT || squareModulus2 <= UtilityFP32.EPSYLON_EFFECTIVENESS_LIMIT)
|
||||
{
|
||||
return squareModulus3 < (2.0f * UtilityFP32.SQUARE_EPSYLON);
|
||||
return squareDistance <= UtilityFP32.SQUARE_EPSYLON;
|
||||
}
|
||||
|
||||
if (squareModulus1 <= squareModulus2)
|
||||
return squareDistance <= UtilityFP32.SQUARE_EPSYLON * squareModulus1 && squareDistance <= UtilityFP32.SQUARE_EPSYLON * squareModulus2;
|
||||
}
|
||||
|
||||
public static bool AreParallel(in Vector2FP32 vector1, in Vector2FP32 vector2)
|
||||
{
|
||||
float squareModulus1 = vector1.GetSquareModulus();
|
||||
float squareModulus2 = vector2.GetSquareModulus();
|
||||
|
||||
if (squareModulus1 <= UtilityFP32.SQUARE_EPSYLON || squareModulus2 <= UtilityFP32.SQUARE_EPSYLON)
|
||||
{
|
||||
return squareModulus3 <= (2.0f * UtilityFP32.SQUARE_EPSYLON) * squareModulus2;
|
||||
return true;
|
||||
}
|
||||
|
||||
return squareModulus3 <= (2.0f * UtilityFP32.SQUARE_EPSYLON) * squareModulus1;
|
||||
float crossProduct = GetCrossProduct(vector1, vector2);
|
||||
|
||||
return crossProduct * crossProduct <= UtilityFP32.SQUARE_EPSYLON * squareModulus1 * squareModulus2;
|
||||
}
|
||||
|
||||
public static bool AreOrthogonal(in Vector2FP32 vector1, in Vector2FP32 vector2)
|
||||
{
|
||||
float squareModulus1 = vector1.GetSquareModulus();
|
||||
float squareModulus2 = vector2.GetSquareModulus();
|
||||
|
||||
if (squareModulus1 <= UtilityFP32.SQUARE_EPSYLON || squareModulus2 <= UtilityFP32.SQUARE_EPSYLON)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
float scalarProduct = GetScalarProduct(vector1, vector2);
|
||||
|
||||
return scalarProduct * scalarProduct <= UtilityFP32.SQUARE_EPSYLON * squareModulus1 * squareModulus2;
|
||||
}
|
||||
|
||||
public static Attitude GetAttitude(in Vector2FP32 vector1, in Vector2FP32 vector2)
|
||||
{
|
||||
float squareModulus1 = vector1.GetSquareModulus();
|
||||
float squareModulus2 = vector2.GetSquareModulus();
|
||||
|
||||
if (squareModulus1 <= UtilityFP32.SQUARE_EPSYLON || squareModulus2 <= UtilityFP32.SQUARE_EPSYLON)
|
||||
{
|
||||
return Attitude.ZERO;
|
||||
}
|
||||
|
||||
float squareLimit = UtilityFP32.SQUARE_EPSYLON * squareModulus1 * squareModulus2;
|
||||
float scalarProduct = GetScalarProduct(vector1, vector2);
|
||||
|
||||
if (scalarProduct * scalarProduct <= squareLimit)
|
||||
{
|
||||
return Attitude.ORTHOGONAL;
|
||||
}
|
||||
|
||||
float crossProduct = GetCrossProduct(vector1, vector2);
|
||||
|
||||
if (crossProduct * crossProduct > squareLimit)
|
||||
{
|
||||
return Attitude.ANY;
|
||||
}
|
||||
|
||||
return scalarProduct > 0.0f ? Attitude.CO_DIRECTIONAL : Attitude.COUNTER_DIRECTIONAL;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -4,7 +4,7 @@
|
|||
* Date: 1 Feb 2019
|
||||
*/
|
||||
|
||||
namespace BasicGeometry
|
||||
namespace BGC
|
||||
{
|
||||
public struct Vector2FP64
|
||||
{
|
||||
|
|
@ -41,35 +41,6 @@ namespace BasicGeometry
|
|||
return Math.Sqrt(this.GetSquareModulus());
|
||||
}
|
||||
|
||||
public int Normalize()
|
||||
{
|
||||
double squareModulus = this.GetSquareModulus();
|
||||
|
||||
if (UtilityFP64.IsSqareUnit(squareModulus))
|
||||
{
|
||||
return 1;
|
||||
}
|
||||
|
||||
if (squareModulus <= UtilityFP64.SQUARE_EPSYLON)
|
||||
{
|
||||
this.Reset();
|
||||
return 0;
|
||||
}
|
||||
|
||||
double multiplier = Math.Sqrt(1.0 / squareModulus);
|
||||
|
||||
this.x1 *= multiplier;
|
||||
this.x2 *= multiplier;
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
public void Reverse()
|
||||
{
|
||||
this.x1 = -this.x1;
|
||||
this.x2 = -this.x2;
|
||||
}
|
||||
|
||||
public readonly bool IsZero()
|
||||
{
|
||||
return this.GetSquareModulus() <= UtilityFP64.SQUARE_EPSYLON;
|
||||
|
|
@ -86,6 +57,39 @@ namespace BasicGeometry
|
|||
this.x2 = 0.0;
|
||||
}
|
||||
|
||||
public void MakeOpposite()
|
||||
{
|
||||
this.x1 = -this.x1;
|
||||
this.x2 = -this.x2;
|
||||
}
|
||||
|
||||
public readonly Vector2FP64 GetOpposite()
|
||||
{
|
||||
return new Vector2FP64(-this.x1, -this.x2);
|
||||
}
|
||||
|
||||
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;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
public void SetValues(double x1, double x2)
|
||||
{
|
||||
this.x1 = x1;
|
||||
|
|
@ -104,27 +108,58 @@ namespace BasicGeometry
|
|||
this.x2 = vector.x2;
|
||||
}
|
||||
|
||||
public void SetReverseOf(in Vector2FP64 vector)
|
||||
{
|
||||
this.x1 = -vector.x1;
|
||||
this.x2 = -vector.x2;
|
||||
}
|
||||
|
||||
public void SetReverseOf(in Vector2FP32 vector)
|
||||
{
|
||||
this.x1 = -vector.x1;
|
||||
this.x2 = -vector.x2;
|
||||
}
|
||||
|
||||
public void AppendScaled(Vector2FP64 summand, double scale)
|
||||
{
|
||||
this.x1 += summand.x1 * scale;
|
||||
this.x2 += summand.x2 * scale;
|
||||
}
|
||||
|
||||
public readonly override string ToString()
|
||||
{
|
||||
return String.Format("DPVector2({0}, {1})", this.x1, this.x2);
|
||||
return String.Format("Vector2FP64({0}, {1})", this.x1, this.x2);
|
||||
}
|
||||
|
||||
public static void Swap(ref Vector2FP64 vector1, ref Vector2FP64 vector2)
|
||||
{
|
||||
double x1 = vector1.x1;
|
||||
double x2 = vector1.x2;
|
||||
|
||||
vector1.x1 = vector2.x1;
|
||||
vector1.x2 = vector2.x2;
|
||||
|
||||
vector2.x1 = x1;
|
||||
vector2.x2 = x2;
|
||||
}
|
||||
|
||||
public static void Reset(out Vector2FP64 vector)
|
||||
{
|
||||
vector.x1 = 0.0;
|
||||
vector.x2 = 0.0;
|
||||
}
|
||||
|
||||
public static void GetOpposite(in Vector2FP64 vector, out Vector2FP64 reverted)
|
||||
{
|
||||
reverted.x1 = -vector.x1;
|
||||
reverted.x2 = -vector.x2;
|
||||
}
|
||||
|
||||
public static bool GetNormalized(in Vector2FP64 vector, out Vector2FP64 normalized)
|
||||
{
|
||||
double squareModulus = vector.GetSquareModulus();
|
||||
|
||||
if (UtilityFP64.IsSqareUnit(squareModulus))
|
||||
{
|
||||
normalized.x1 = vector.x1;
|
||||
normalized.x2 = vector.x2;
|
||||
return true;
|
||||
}
|
||||
|
||||
if (squareModulus <= UtilityFP64.SQUARE_EPSYLON || double.IsNaN(squareModulus))
|
||||
{
|
||||
normalized.x1 = 0.0;
|
||||
normalized.x2 = 0.0;
|
||||
return false;
|
||||
}
|
||||
|
||||
double multiplier = Math.Sqrt(1.0 / squareModulus);
|
||||
|
||||
normalized.x1 = vector.x1 * multiplier;
|
||||
normalized.x2 = vector.x2 * multiplier;
|
||||
return true;
|
||||
}
|
||||
|
||||
public static void Add(in Vector2FP64 vector1, in Vector2FP64 vector2, out Vector2FP64 sum)
|
||||
|
|
@ -133,6 +168,12 @@ namespace BasicGeometry
|
|||
sum.x2 = vector1.x2 + vector2.x2;
|
||||
}
|
||||
|
||||
public static void AddScaled(in Vector2FP64 basicVector, in Vector2FP64 scalableVector, double scale, out Vector2FP64 sum)
|
||||
{
|
||||
sum.x1 = basicVector.x1 + scalableVector.x1 * scale;
|
||||
sum.x2 = basicVector.x2 + scalableVector.x2 * scale;
|
||||
}
|
||||
|
||||
public static void Subtract(in Vector2FP64 minuend, in Vector2FP64 subtrahend, out Vector2FP64 difference)
|
||||
{
|
||||
difference.x1 = minuend.x1 - subtrahend.x1;
|
||||
|
|
@ -150,16 +191,24 @@ namespace BasicGeometry
|
|||
Multiply(dividend, 1.0 / divisor, out quotient);
|
||||
}
|
||||
|
||||
public static void GetMean2(in Vector2FP64 vector1, in Vector2FP64 vector2, out Vector2FP64 result)
|
||||
public static void GetMeanOfTwo(in Vector2FP64 vector1, in Vector2FP64 vector2, out Vector2FP64 mean)
|
||||
{
|
||||
result.x1 = (vector1.x1 + vector2.x1) * 0.5;
|
||||
result.x2 = (vector1.x2 + vector2.x2) * 0.5;
|
||||
mean.x1 = (vector1.x1 + vector2.x1) * 0.5;
|
||||
mean.x2 = (vector1.x2 + vector2.x2) * 0.5;
|
||||
}
|
||||
|
||||
public static void GetMean3(in Vector2FP64 vector1, in Vector2FP64 vector2, in Vector2FP64 vector3, out Vector2FP64 result)
|
||||
public static void GetMeanOfThree(in Vector2FP64 vector1, in Vector2FP64 vector2, in Vector2FP64 vector3, out Vector2FP64 mean)
|
||||
{
|
||||
result.x1 = (vector1.x1 + vector2.x1 + vector3.x1) * UtilityFP64.ONE_THIRD;
|
||||
result.x2 = (vector1.x2 + vector2.x2 + vector3.x2) * UtilityFP64.ONE_THIRD;
|
||||
mean.x1 = (vector1.x1 + vector2.x1 + vector3.x1) * UtilityFP64.ONE_THIRD;
|
||||
mean.x2 = (vector1.x2 + vector2.x2 + vector3.x2) * UtilityFP64.ONE_THIRD;
|
||||
}
|
||||
|
||||
public static void Interpolate(in Vector2FP64 vector1, in Vector2FP64 vector2, double phase, out Vector2FP64 interpolation)
|
||||
{
|
||||
double counterphase = 1.0 - phase;
|
||||
|
||||
interpolation.x1 = vector1.x1 * counterphase + vector2.x1 * phase;
|
||||
interpolation.x2 = vector1.x2 * counterphase + vector2.x2 * phase;
|
||||
}
|
||||
|
||||
public static double GetScalarProduct(in Vector2FP64 vector1, in Vector2FP64 vector2)
|
||||
|
|
@ -176,31 +225,24 @@ namespace BasicGeometry
|
|||
{
|
||||
double squareModulus1 = vector1.GetSquareModulus();
|
||||
|
||||
if (squareModulus1 <= UtilityFP64.SQUARE_EPSYLON)
|
||||
if (squareModulus1 <= UtilityFP64.SQUARE_EPSYLON || double.IsNaN(squareModulus1))
|
||||
{
|
||||
return 0.0;
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
double squareModulus2 = vector2.GetSquareModulus();
|
||||
|
||||
if (squareModulus2 <= UtilityFP64.SQUARE_EPSYLON)
|
||||
if (squareModulus2 <= UtilityFP64.SQUARE_EPSYLON || double.IsNaN(squareModulus2))
|
||||
{
|
||||
return 0.0;
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
double cosine = Vector2FP64.GetScalarProduct(vector1, vector2) / Math.Sqrt(squareModulus1 * squareModulus2);
|
||||
double multiplier = Math.Sqrt(1.0 / (squareModulus1 * squareModulus2));
|
||||
|
||||
if (1.0 - UtilityFP64.EPSYLON <= cosine)
|
||||
{
|
||||
return 0.0;
|
||||
}
|
||||
double x = GetScalarProduct(vector1, vector2);
|
||||
double y = GetCrossProduct(vector1, vector2);
|
||||
|
||||
if (cosine <= -(1.0 - UtilityFP64.EPSYLON))
|
||||
{
|
||||
return AngleFP64.GetHalfCircle(unit);
|
||||
}
|
||||
|
||||
return RadianFP64.ToUnits(Math.Acos(cosine), unit);
|
||||
return RadianFP64.ToUnits(Math.Atan2(y * multiplier, x * multiplier), unit);
|
||||
}
|
||||
|
||||
public static double GetSquareDistance(in Vector2FP64 vector1, in Vector2FP64 vector2)
|
||||
|
|
@ -215,25 +257,82 @@ namespace BasicGeometry
|
|||
{
|
||||
return Math.Sqrt(GetSquareDistance(vector1, vector2));
|
||||
}
|
||||
|
||||
public static bool AreEqual(in Vector2FP64 vector1, in Vector2FP64 vector2)
|
||||
|
||||
public static bool AreCloseEnough(in Vector2FP64 vector1, in Vector2FP64 vector2, double distanceLimit)
|
||||
{
|
||||
return 0.0 <= distanceLimit && GetSquareDistance(vector1, vector2) <= distanceLimit * distanceLimit;
|
||||
}
|
||||
|
||||
public static bool AreClose(in Vector2FP64 vector1, in Vector2FP64 vector2)
|
||||
{
|
||||
double squareModulus1 = vector1.GetSquareModulus();
|
||||
double squareModulus2 = vector2.GetSquareModulus();
|
||||
double squareModulus3 = GetSquareDistance(vector1, vector2);
|
||||
double squareDistance = GetSquareDistance(vector1, vector2);
|
||||
|
||||
// 2.0 means dimension amount
|
||||
if (squareModulus1 < UtilityFP64.EPSYLON_EFFECTIVENESS_LIMIT || squareModulus2 < UtilityFP64.EPSYLON_EFFECTIVENESS_LIMIT)
|
||||
if (squareModulus1 <= UtilityFP64.EPSYLON_EFFECTIVENESS_LIMIT || squareModulus2 <= UtilityFP64.EPSYLON_EFFECTIVENESS_LIMIT)
|
||||
{
|
||||
return squareModulus3 < (2.0 * UtilityFP64.SQUARE_EPSYLON);
|
||||
return squareDistance <= UtilityFP64.SQUARE_EPSYLON;
|
||||
}
|
||||
|
||||
if (squareModulus1 <= squareModulus2)
|
||||
return squareDistance <= UtilityFP64.SQUARE_EPSYLON * squareModulus1 && squareDistance <= UtilityFP64.SQUARE_EPSYLON * squareModulus2;
|
||||
}
|
||||
|
||||
public static bool AreParallel(in Vector2FP64 vector1, in Vector2FP64 vector2)
|
||||
{
|
||||
double squareModulus1 = vector1.GetSquareModulus();
|
||||
double squareModulus2 = vector2.GetSquareModulus();
|
||||
|
||||
if (squareModulus1 <= UtilityFP64.SQUARE_EPSYLON || squareModulus2 <= UtilityFP64.SQUARE_EPSYLON)
|
||||
{
|
||||
return squareModulus3 <= (2.0 * UtilityFP64.SQUARE_EPSYLON) * squareModulus2;
|
||||
return true;
|
||||
}
|
||||
|
||||
return squareModulus3 <= (2.0 * UtilityFP64.SQUARE_EPSYLON) * squareModulus1;
|
||||
double crossProduct = GetCrossProduct(vector1, vector2);
|
||||
|
||||
return crossProduct * crossProduct <= UtilityFP64.SQUARE_EPSYLON * squareModulus1 * squareModulus2;
|
||||
}
|
||||
|
||||
public static bool AreOrthogonal(in Vector2FP64 vector1, in Vector2FP64 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 Vector2FP64 vector1, in Vector2FP64 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;
|
||||
}
|
||||
|
||||
double crossProduct = GetCrossProduct(vector1, vector2);
|
||||
|
||||
if (crossProduct * crossProduct > squareLimit)
|
||||
{
|
||||
return Attitude.ANY;
|
||||
}
|
||||
|
||||
return scalarProduct > 0.0 ? Attitude.CO_DIRECTIONAL : Attitude.COUNTER_DIRECTIONAL;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -4,7 +4,7 @@
|
|||
* Date: 1 Feb 2019
|
||||
*/
|
||||
|
||||
namespace BasicGeometry
|
||||
namespace BGC
|
||||
{
|
||||
public struct Vector3FP32
|
||||
{
|
||||
|
|
@ -45,37 +45,6 @@ namespace BasicGeometry
|
|||
return MathF.Sqrt(this.GetSquareModulus());
|
||||
}
|
||||
|
||||
public int Normalize()
|
||||
{
|
||||
float squareModulus = this.GetSquareModulus();
|
||||
|
||||
if (UtilityFP32.IsSqareUnit(squareModulus))
|
||||
{
|
||||
return 1;
|
||||
}
|
||||
|
||||
if (squareModulus <= UtilityFP32.SQUARE_EPSYLON)
|
||||
{
|
||||
this.Reset();
|
||||
return 0;
|
||||
}
|
||||
|
||||
float multiplier = MathF.Sqrt(1.0f / squareModulus);
|
||||
|
||||
this.x1 *= multiplier;
|
||||
this.x2 *= multiplier;
|
||||
this.x3 *= multiplier;
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
public void Reverse()
|
||||
{
|
||||
this.x1 = -this.x1;
|
||||
this.x2 = -this.x2;
|
||||
this.x3 = -this.x3;
|
||||
}
|
||||
|
||||
public readonly bool IsZero()
|
||||
{
|
||||
return this.GetSquareModulus() <= UtilityFP32.SQUARE_EPSYLON;
|
||||
|
|
@ -93,6 +62,41 @@ namespace BasicGeometry
|
|||
this.x3 = 0.0f;
|
||||
}
|
||||
|
||||
public void MakeOpposite()
|
||||
{
|
||||
this.x1 = -this.x1;
|
||||
this.x2 = -this.x2;
|
||||
this.x3 = -this.x3;
|
||||
}
|
||||
|
||||
public readonly Vector3FP32 GetOpposite()
|
||||
{
|
||||
return new Vector3FP32(-this.x1, -this.x2, -this.x3);
|
||||
}
|
||||
|
||||
public bool Normalize()
|
||||
{
|
||||
float squareModulus = this.GetSquareModulus();
|
||||
|
||||
if (UtilityFP32.IsSqareUnit(squareModulus))
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
if (squareModulus <= UtilityFP32.SQUARE_EPSYLON || float.IsNaN(squareModulus))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
float multiplier = MathF.Sqrt(1.0f / squareModulus);
|
||||
|
||||
this.x1 *= multiplier;
|
||||
this.x2 *= multiplier;
|
||||
this.x3 *= multiplier;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
public void SetValues(float x1, float x2, float x3)
|
||||
{
|
||||
this.x1 = x1;
|
||||
|
|
@ -114,30 +118,66 @@ namespace BasicGeometry
|
|||
this.x3 = vector.x3;
|
||||
}
|
||||
|
||||
public void SetReverseOf(in Vector3FP32 vector)
|
||||
{
|
||||
this.x1 = -vector.x1;
|
||||
this.x2 = -vector.x2;
|
||||
this.x3 = -vector.x3;
|
||||
}
|
||||
|
||||
public void SetReverseOf(in Vector3FP64 vector)
|
||||
{
|
||||
this.x1 = -(float)vector.x1;
|
||||
this.x2 = -(float)vector.x2;
|
||||
this.x3 = -(float)vector.x3;
|
||||
}
|
||||
|
||||
public void AppendScaled(Vector3FP32 summand, float scale)
|
||||
{
|
||||
this.x1 += summand.x1 * scale;
|
||||
this.x2 += summand.x2 * scale;
|
||||
this.x3 += summand.x3 * scale;
|
||||
}
|
||||
|
||||
public readonly override string ToString()
|
||||
{
|
||||
return String.Format("SPVector3({0}, {1}, {2})", this.x1, this.x2, this.x3);
|
||||
return String.Format("Vector3FP32({0}, {1}, {2})", this.x1, this.x2, this.x3);
|
||||
}
|
||||
|
||||
public static void Swap(ref Vector3FP32 vector1, ref Vector3FP32 vector2)
|
||||
{
|
||||
float x1 = vector1.x1;
|
||||
float x2 = vector1.x2;
|
||||
float 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 Vector3FP32 vector)
|
||||
{
|
||||
vector.x1 = 0.0f;
|
||||
vector.x2 = 0.0f;
|
||||
vector.x3 = 0.0f;
|
||||
}
|
||||
|
||||
public static void GetOpposite(in Vector3FP32 vector, out Vector3FP32 reverted)
|
||||
{
|
||||
reverted.x1 = -vector.x1;
|
||||
reverted.x2 = -vector.x2;
|
||||
reverted.x3 = -vector.x3;
|
||||
}
|
||||
|
||||
public static bool GetNormalized(in Vector3FP32 vector, out Vector3FP32 normalized)
|
||||
{
|
||||
float squareModulus = vector.GetSquareModulus();
|
||||
|
||||
if (UtilityFP32.IsSqareUnit(squareModulus))
|
||||
{
|
||||
normalized.x1 = vector.x1;
|
||||
normalized.x2 = vector.x2;
|
||||
normalized.x3 = vector.x3;
|
||||
return true;
|
||||
}
|
||||
|
||||
if (squareModulus <= UtilityFP32.SQUARE_EPSYLON || float.IsNaN(squareModulus))
|
||||
{
|
||||
normalized.x1 = 0.0f;
|
||||
normalized.x2 = 0.0f;
|
||||
normalized.x3 = 0.0f;
|
||||
return false;
|
||||
}
|
||||
|
||||
float multiplier = MathF.Sqrt(1.0f / squareModulus);
|
||||
|
||||
normalized.x1 = vector.x1 * multiplier;
|
||||
normalized.x2 = vector.x2 * multiplier;
|
||||
normalized.x3 = vector.x3 * multiplier;
|
||||
return true;
|
||||
}
|
||||
|
||||
public static void Add(in Vector3FP32 vector1, in Vector3FP32 vector2, out Vector3FP32 sum)
|
||||
|
|
@ -147,6 +187,13 @@ namespace BasicGeometry
|
|||
sum.x3 = vector1.x3 + vector2.x3;
|
||||
}
|
||||
|
||||
public static void AddScaled(in Vector3FP32 basicVector, in Vector3FP32 scalableVector, float scale, out Vector3FP32 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 Vector3FP32 minuend, in Vector3FP32 subtrahend, out Vector3FP32 difference)
|
||||
{
|
||||
difference.x1 = minuend.x1 - subtrahend.x1;
|
||||
|
|
@ -166,35 +213,33 @@ namespace BasicGeometry
|
|||
Multiply(dividend, 1.0f / divisor, out quotient);
|
||||
}
|
||||
|
||||
public static void GetMean2(in Vector3FP32 vector1, in Vector3FP32 vector2, out Vector3FP32 result)
|
||||
public static void GetMeanOfTwo(in Vector3FP32 vector1, in Vector3FP32 vector2, out Vector3FP32 mean)
|
||||
{
|
||||
result.x1 = (vector1.x1 + vector2.x1) * 0.5f;
|
||||
result.x2 = (vector1.x2 + vector2.x2) * 0.5f;
|
||||
result.x3 = (vector1.x3 + vector2.x3) * 0.5f;
|
||||
mean.x1 = (vector1.x1 + vector2.x1) * 0.5f;
|
||||
mean.x2 = (vector1.x2 + vector2.x2) * 0.5f;
|
||||
mean.x3 = (vector1.x3 + vector2.x3) * 0.5f;
|
||||
}
|
||||
|
||||
public static void GetMean3(in Vector3FP32 vector1, in Vector3FP32 vector2, in Vector3FP32 vector3, out Vector3FP32 result)
|
||||
public static void GetMeanOfThree(in Vector3FP32 vector1, in Vector3FP32 vector2, in Vector3FP32 vector3, out Vector3FP32 mean)
|
||||
{
|
||||
result.x1 = (vector1.x1 + vector2.x1 + vector3.x1) * UtilityFP32.ONE_THIRD;
|
||||
result.x2 = (vector1.x2 + vector2.x2 + vector3.x2) * UtilityFP32.ONE_THIRD;
|
||||
result.x3 = (vector1.x3 + vector2.x3 + vector3.x3) * UtilityFP32.ONE_THIRD;
|
||||
mean.x1 = (vector1.x1 + vector2.x1 + vector3.x1) * UtilityFP32.ONE_THIRD;
|
||||
mean.x2 = (vector1.x2 + vector2.x2 + vector3.x2) * UtilityFP32.ONE_THIRD;
|
||||
mean.x3 = (vector1.x3 + vector2.x3 + vector3.x3) * UtilityFP32.ONE_THIRD;
|
||||
}
|
||||
|
||||
public static void Interpolate(in Vector3FP32 vector1, in Vector3FP32 vector2, float phase, out Vector3FP32 interpolation)
|
||||
{
|
||||
float counterphase = 1.0f - 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 float GetScalarProduct(in Vector3FP32 vector1, in Vector3FP32 vector2)
|
||||
{
|
||||
return vector1.x1 * vector2.x1 + vector1.x2 * vector2.x2 + vector1.x3 * vector2.x3;
|
||||
}
|
||||
|
||||
public static void GetCrossProduct(in Vector3FP32 vector1, in Vector3FP32 vector2, out Vector3FP32 result)
|
||||
{
|
||||
float x1 = vector1.x2 * vector2.x3 - vector1.x3 * vector2.x2;
|
||||
float x2 = vector1.x3 * vector2.x1 - vector1.x1 * vector2.x3;
|
||||
float x3 = vector1.x1 * vector2.x2 - vector1.x2 * vector2.x1;
|
||||
|
||||
result.x1 = x1;
|
||||
result.x2 = x2;
|
||||
result.x3 = x3;
|
||||
}
|
||||
|
||||
public static float GetTripleProduct(in Vector3FP32 vector1, in Vector3FP32 vector2, in Vector3FP32 vector3)
|
||||
{
|
||||
|
|
@ -202,47 +247,55 @@ namespace BasicGeometry
|
|||
+ vector1.x2 * (vector2.x3 * vector3.x1 - vector2.x1 * vector3.x3)
|
||||
+ vector1.x3 * (vector2.x1 * vector3.x2 - vector2.x2 * vector3.x1);
|
||||
}
|
||||
|
||||
public static void GetDoubleCrossProduct(in Vector3FP32 vector1, in Vector3FP32 vector2, in Vector3FP32 vector3, out Vector3FP32 result)
|
||||
|
||||
public static void GetCrossProduct(in Vector3FP32 vector1, in Vector3FP32 vector2, out Vector3FP32 product)
|
||||
{
|
||||
float x1 = vector1.x2 * vector2.x3 - vector1.x3 * vector2.x2;
|
||||
float x2 = vector1.x3 * vector2.x1 - vector1.x1 * vector2.x3;
|
||||
float x3 = vector1.x1 * vector2.x2 - vector1.x2 * vector2.x1;
|
||||
|
||||
product.x1 = x1;
|
||||
product.x2 = x2;
|
||||
product.x3 = x3;
|
||||
}
|
||||
|
||||
// [a x [b x c]] = b * (a, c) - c * (a, b)
|
||||
public static void GetDoubleCrossProduct(in Vector3FP32 vector1, in Vector3FP32 vector2, in Vector3FP32 vector3, out Vector3FP32 product)
|
||||
{
|
||||
// [a x [b x c]] = b * (a, c) - c * (a, b)
|
||||
float ac = GetScalarProduct(vector1, vector3);
|
||||
float ab = GetScalarProduct(vector1, vector2);
|
||||
|
||||
result.x1 = ac * vector2.x1 - ab * vector3.x1;
|
||||
result.x2 = ac * vector2.x2 - ab * vector3.x2;
|
||||
result.x3 = ac * vector2.x3 - ab * vector3.x3;
|
||||
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 float GetAngle(in Vector3FP32 vector1, in Vector3FP32 vector2, AngleUnit unit)
|
||||
{
|
||||
float squareModulus1 = vector1.GetSquareModulus();
|
||||
|
||||
if (squareModulus1 <= UtilityFP32.SQUARE_EPSYLON)
|
||||
if (squareModulus1 <= UtilityFP32.SQUARE_EPSYLON || float.IsNaN(squareModulus1))
|
||||
{
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
float squareModulus2 = vector2.GetSquareModulus();
|
||||
|
||||
if (squareModulus2 <= UtilityFP32.SQUARE_EPSYLON)
|
||||
if (squareModulus2 <= UtilityFP32.SQUARE_EPSYLON || float.IsNaN(squareModulus2))
|
||||
{
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
float cosine = Vector3FP32.GetScalarProduct(vector1, vector2) / MathF.Sqrt(squareModulus1 * squareModulus2);
|
||||
float multiplier = MathF.Sqrt(1.0f / (squareModulus1 * squareModulus2));
|
||||
|
||||
if (1.0f - UtilityFP32.EPSYLON <= cosine)
|
||||
{
|
||||
return 0.0f;
|
||||
}
|
||||
Vector3FP32 crossProduct;
|
||||
|
||||
if (cosine <= -(1.0f - UtilityFP32.EPSYLON))
|
||||
{
|
||||
return AngleFP32.GetHalfCircle(unit);
|
||||
}
|
||||
|
||||
return RadianFP32.ToUnits(MathF.Acos(cosine), unit);
|
||||
GetCrossProduct(vector1, vector2, out crossProduct);
|
||||
|
||||
float x = GetScalarProduct(vector1, vector2);
|
||||
float y = crossProduct.GetModulus();
|
||||
|
||||
return RadianFP32.ToUnits(MathF.Atan2(y * multiplier, x * multiplier), unit);
|
||||
}
|
||||
|
||||
public static float GetSquareDistance(in Vector3FP32 vector1, in Vector3FP32 vector2)
|
||||
|
|
@ -258,14 +311,18 @@ namespace BasicGeometry
|
|||
{
|
||||
return MathF.Sqrt(GetSquareDistance(vector1, vector2));
|
||||
}
|
||||
|
||||
public static bool AreEqual(in Vector3FP32 vector1, in Vector3FP32 vector2)
|
||||
|
||||
public static bool AreCloseEnough(in Vector3FP32 vector1, in Vector3FP32 vector2, float distanceLimit)
|
||||
{
|
||||
return 0.0f <= distanceLimit && GetSquareDistance(vector1, vector2) <= distanceLimit * distanceLimit;
|
||||
}
|
||||
|
||||
public static bool AreClose(in Vector3FP32 vector1, in Vector3FP32 vector2)
|
||||
{
|
||||
float squareModulus1 = vector1.GetSquareModulus();
|
||||
float squareModulus2 = vector2.GetSquareModulus();
|
||||
float squareDistance = GetSquareDistance(vector1, vector2);
|
||||
|
||||
// 3.0f means dimension amount
|
||||
if (squareModulus1 <= UtilityFP32.EPSYLON_EFFECTIVENESS_LIMIT || squareModulus2 <= UtilityFP32.EPSYLON_EFFECTIVENESS_LIMIT)
|
||||
{
|
||||
return squareDistance <= UtilityFP32.SQUARE_EPSYLON;
|
||||
|
|
@ -273,5 +330,67 @@ namespace BasicGeometry
|
|||
|
||||
return squareDistance <= UtilityFP32.SQUARE_EPSYLON * squareModulus1 && squareDistance <= UtilityFP32.SQUARE_EPSYLON * squareModulus2;
|
||||
}
|
||||
|
||||
public static bool AreParallel(in Vector3FP32 vector1, in Vector3FP32 vector2)
|
||||
{
|
||||
float squareModulus1 = vector1.GetSquareModulus();
|
||||
float squareModulus2 = vector2.GetSquareModulus();
|
||||
|
||||
if (squareModulus1 <= UtilityFP32.SQUARE_EPSYLON || squareModulus2 <= UtilityFP32.SQUARE_EPSYLON)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
Vector3FP32 crossProduct;
|
||||
|
||||
GetCrossProduct(vector1, vector2, out crossProduct);
|
||||
|
||||
return crossProduct.GetSquareModulus() <= UtilityFP32.SQUARE_EPSYLON * squareModulus1 * squareModulus2;
|
||||
}
|
||||
|
||||
public static bool AreOrthogonal(in Vector3FP32 vector1, in Vector3FP32 vector2)
|
||||
{
|
||||
float squareModulus1 = vector1.GetSquareModulus();
|
||||
float squareModulus2 = vector2.GetSquareModulus();
|
||||
|
||||
if (squareModulus1 <= UtilityFP32.SQUARE_EPSYLON || squareModulus2 <= UtilityFP32.SQUARE_EPSYLON)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
float scalarProduct = GetScalarProduct(vector1, vector2);
|
||||
|
||||
return scalarProduct * scalarProduct <= UtilityFP32.SQUARE_EPSYLON * squareModulus1 * squareModulus2;
|
||||
}
|
||||
|
||||
public static Attitude GetAttitude(in Vector3FP32 vector1, in Vector3FP32 vector2)
|
||||
{
|
||||
float squareModulus1 = vector1.GetSquareModulus();
|
||||
float squareModulus2 = vector2.GetSquareModulus();
|
||||
|
||||
if (squareModulus1 <= UtilityFP32.SQUARE_EPSYLON || squareModulus2 <= UtilityFP32.SQUARE_EPSYLON)
|
||||
{
|
||||
return Attitude.ZERO;
|
||||
}
|
||||
|
||||
float squareLimit = UtilityFP32.SQUARE_EPSYLON * squareModulus1 * squareModulus2;
|
||||
float scalarProduct = GetScalarProduct(vector1, vector2);
|
||||
|
||||
if (scalarProduct * scalarProduct <= squareLimit)
|
||||
{
|
||||
return Attitude.ORTHOGONAL;
|
||||
}
|
||||
|
||||
Vector3FP32 crossProduct;
|
||||
|
||||
GetCrossProduct(vector1, vector2, out crossProduct);
|
||||
|
||||
if (crossProduct.GetSquareModulus() > squareLimit)
|
||||
{
|
||||
return Attitude.ANY;
|
||||
}
|
||||
|
||||
return scalarProduct > 0.0f ? Attitude.CO_DIRECTIONAL : Attitude.COUNTER_DIRECTIONAL;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -4,7 +4,7 @@
|
|||
* Date: 1 Feb 2019
|
||||
*/
|
||||
|
||||
namespace BasicGeometry
|
||||
namespace BGC
|
||||
{
|
||||
public struct Vector3FP64
|
||||
{
|
||||
|
|
@ -45,38 +45,6 @@ namespace BasicGeometry
|
|||
return Math.Sqrt(this.GetSquareModulus());
|
||||
}
|
||||
|
||||
public int Normalize()
|
||||
{
|
||||
double squareModulus = this.GetSquareModulus();
|
||||
|
||||
|
||||
if (UtilityFP64.IsSqareUnit(squareModulus))
|
||||
{
|
||||
return 1;
|
||||
}
|
||||
|
||||
if (squareModulus <= UtilityFP64.SQUARE_EPSYLON)
|
||||
{
|
||||
this.Reset();
|
||||
return 0;
|
||||
}
|
||||
|
||||
double multiplier = Math.Sqrt(1.0 / squareModulus);
|
||||
|
||||
this.x1 *= multiplier;
|
||||
this.x2 *= multiplier;
|
||||
this.x3 *= multiplier;
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
public void Reverse()
|
||||
{
|
||||
this.x1 = -this.x1;
|
||||
this.x2 = -this.x2;
|
||||
this.x3 = -this.x3;
|
||||
}
|
||||
|
||||
public readonly bool IsZero()
|
||||
{
|
||||
return this.GetSquareModulus() <= UtilityFP64.SQUARE_EPSYLON;
|
||||
|
|
@ -94,6 +62,41 @@ namespace BasicGeometry
|
|||
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;
|
||||
|
|
@ -115,30 +118,66 @@ namespace BasicGeometry
|
|||
this.x3 = vector.x3;
|
||||
}
|
||||
|
||||
public void SetReverseOf(in Vector3FP64 vector)
|
||||
{
|
||||
this.x1 = -vector.x1;
|
||||
this.x2 = -vector.x2;
|
||||
this.x3 = -vector.x3;
|
||||
}
|
||||
|
||||
public void SetReverseOf(in Vector3FP32 vector)
|
||||
{
|
||||
this.x1 = -vector.x1;
|
||||
this.x2 = -vector.x2;
|
||||
this.x3 = -vector.x3;
|
||||
}
|
||||
|
||||
public void AppendScaled(Vector3FP64 summand, double scale)
|
||||
{
|
||||
this.x1 += summand.x1 * scale;
|
||||
this.x2 += summand.x2 * scale;
|
||||
this.x3 += summand.x3 * scale;
|
||||
}
|
||||
|
||||
public readonly override string ToString()
|
||||
{
|
||||
return String.Format("DPVector3({0}, {1}, {2})", this.x1, this.x2, this.x3);
|
||||
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)
|
||||
|
|
@ -148,6 +187,13 @@ namespace BasicGeometry
|
|||
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;
|
||||
|
|
@ -167,25 +213,41 @@ namespace BasicGeometry
|
|||
Multiply(dividend, 1.0 / divisor, out quotient);
|
||||
}
|
||||
|
||||
public static void GetMean2(in Vector3FP64 vector1, in Vector3FP64 vector2, out Vector3FP64 result)
|
||||
public static void GetMeanOfTwo(in Vector3FP64 vector1, in Vector3FP64 vector2, out Vector3FP64 mean)
|
||||
{
|
||||
result.x1 = (vector1.x1 + vector2.x1) * 0.5;
|
||||
result.x2 = (vector1.x2 + vector2.x2) * 0.5;
|
||||
result.x3 = (vector1.x3 + vector2.x3) * 0.5;
|
||||
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 GetMean3(in Vector3FP64 vector1, in Vector3FP64 vector2, in Vector3FP64 vector3, out Vector3FP64 result)
|
||||
public static void GetMeanOfThree(in Vector3FP64 vector1, in Vector3FP64 vector2, in Vector3FP64 vector3, out Vector3FP64 mean)
|
||||
{
|
||||
result.x1 = (vector1.x1 + vector2.x1 + vector3.x1) * UtilityFP64.ONE_THIRD;
|
||||
result.x2 = (vector1.x2 + vector2.x2 + vector3.x2) * UtilityFP64.ONE_THIRD;
|
||||
result.x3 = (vector1.x3 + vector2.x3 + vector3.x3) * UtilityFP64.ONE_THIRD;
|
||||
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;
|
||||
|
|
@ -197,55 +259,45 @@ namespace BasicGeometry
|
|||
result.x3 = x3;
|
||||
}
|
||||
|
||||
public static double GetTripleProduct(in Vector3FP64 vector1, in Vector3FP64 vector2, in Vector3FP64 vector3)
|
||||
// [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)
|
||||
{
|
||||
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 GetDoubleCrossProduct(in Vector3FP64 vector1, in Vector3FP64 vector2, in Vector3FP64 vector3, out Vector3FP64 result)
|
||||
{
|
||||
// [a x [b x c]] = b * (a, c) - c * (a, b)
|
||||
double ac = GetScalarProduct(vector1, vector3);
|
||||
double ab = GetScalarProduct(vector1, vector2);
|
||||
|
||||
result.x1 = ac * vector2.x1 - ab * vector3.x1;
|
||||
result.x2 = ac * vector2.x2 - ab * vector3.x2;
|
||||
result.x3 = ac * vector2.x3 - ab * vector3.x3;
|
||||
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)
|
||||
if (squareModulus1 <= UtilityFP64.SQUARE_EPSYLON || double.IsNaN(squareModulus1))
|
||||
{
|
||||
return 0.0;
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
double squareModulus2 = vector2.GetSquareModulus();
|
||||
|
||||
if (squareModulus2 <= UtilityFP64.SQUARE_EPSYLON)
|
||||
if (squareModulus2 <= UtilityFP64.SQUARE_EPSYLON || double.IsNaN(squareModulus2))
|
||||
{
|
||||
return 0.0;
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
double cosine = Vector3FP64.GetScalarProduct(vector1, vector2) / Math.Sqrt(squareModulus1 * squareModulus2);
|
||||
double multiplier = Math.Sqrt(1.0 / (squareModulus1 * squareModulus2));
|
||||
|
||||
if (1.0 - UtilityFP64.EPSYLON <= cosine)
|
||||
{
|
||||
return 0.0;
|
||||
}
|
||||
Vector3FP64 crossProduct;
|
||||
|
||||
if (cosine <= -(1.0 - UtilityFP64.EPSYLON))
|
||||
{
|
||||
return AngleFP64.GetHalfCircle(unit);
|
||||
}
|
||||
|
||||
return RadianFP64.ToUnits(Math.Acos(cosine), unit);
|
||||
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;
|
||||
|
|
@ -259,8 +311,13 @@ namespace BasicGeometry
|
|||
{
|
||||
return Math.Sqrt(GetSquareDistance(vector1, vector2));
|
||||
}
|
||||
|
||||
public static bool AreEqual(in Vector3FP64 vector1, in Vector3FP64 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();
|
||||
|
|
@ -273,5 +330,67 @@ namespace BasicGeometry
|
|||
|
||||
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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -4,7 +4,7 @@
|
|||
* Date: 20 Oct 2024
|
||||
*/
|
||||
|
||||
namespace BasicGeometry
|
||||
namespace BGC
|
||||
{
|
||||
public struct VersorFP32
|
||||
{
|
||||
|
|
@ -98,6 +98,14 @@ namespace BasicGeometry
|
|||
this.x3 = 0.0f;
|
||||
}
|
||||
|
||||
public void MakeOpposite()
|
||||
{
|
||||
this.s0 = -this.s0;
|
||||
this.x1 = -this.x1;
|
||||
this.x2 = -this.x2;
|
||||
this.x3 = -this.x3;
|
||||
}
|
||||
|
||||
public void Shorten()
|
||||
{
|
||||
if (this.s0 < 0.0f)
|
||||
|
|
@ -162,25 +170,43 @@ namespace BasicGeometry
|
|||
|
||||
public static void Combine(in VersorFP32 second, in VersorFP32 first, out VersorFP32 result)
|
||||
{
|
||||
float s0 = second.s0 * first.s0 - second.x1 * first.x1 - (second.x2 * first.x2 + second.x3 * first.x3);
|
||||
float x1 = second.x1 * first.s0 + second.s0 * first.x1 - (second.x3 * first.x2 - second.x2 * first.x3);
|
||||
float x2 = second.x2 * first.s0 + second.s0 * first.x2 - (second.x1 * first.x3 - second.x3 * first.x1);
|
||||
float x3 = second.x3 * first.s0 + second.s0 * first.x3 - (second.x2 * first.x1 - second.x1 * first.x2);
|
||||
|
||||
float squareModulus = s0 * s0 + x1 * x1 + (x2 * x2 + x3 * x3);
|
||||
|
||||
result.s0 = s0;
|
||||
result.x1 = x1;
|
||||
result.x2 = x2;
|
||||
result.x3 = x3;
|
||||
|
||||
if (!UtilityFP32.IsSqareUnit(squareModulus))
|
||||
{
|
||||
result.Normalize(squareModulus);
|
||||
}
|
||||
LoadValues(
|
||||
(second.s0 * first.s0 - second.x1 * first.x1) - (second.x2 * first.x2 + second.x3 * first.x3),
|
||||
(second.x1 * first.s0 + second.s0 * first.x1) - (second.x3 * first.x2 - second.x2 * first.x3),
|
||||
(second.x2 * first.s0 + second.s0 * first.x2) - (second.x1 * first.x3 - second.x3 * first.x1),
|
||||
(second.x3 * first.s0 + second.s0 * first.x3) - (second.x2 * first.x1 - second.x1 * first.x2),
|
||||
out result
|
||||
);
|
||||
}
|
||||
|
||||
public static void MakeInverted(in VersorFP32 versor, out VersorFP32 conjugate)
|
||||
public static void Combine(in VersorFP32 third, in VersorFP32 second, in VersorFP32 first, out VersorFP32 result)
|
||||
{
|
||||
float s0 = (second.s0 * first.s0 - second.x1 * first.x1) - (second.x2 * first.x2 + second.x3 * first.x3);
|
||||
float x1 = (second.x1 * first.s0 + second.s0 * first.x1) - (second.x3 * first.x2 - second.x2 * first.x3);
|
||||
float x2 = (second.x2 * first.s0 + second.s0 * first.x2) - (second.x1 * first.x3 - second.x3 * first.x1);
|
||||
float x3 = (second.x3 * first.s0 + second.s0 * first.x3) - (second.x2 * first.x1 - second.x1 * first.x2);
|
||||
|
||||
LoadValues(
|
||||
(third.s0 * s0 - third.x1 * x1) - (third.x2 * x2 + third.x3 * x3),
|
||||
(third.x1 * s0 + third.s0 * x1) - (third.x3 * x2 - third.x2 * x3),
|
||||
(third.x2 * s0 + third.s0 * x2) - (third.x1 * x3 - third.x3 * x1),
|
||||
(third.x3 * s0 + third.s0 * x3) - (third.x2 * x1 - third.x1 * x2),
|
||||
out result
|
||||
);
|
||||
}
|
||||
|
||||
public static void Exclude(in VersorFP32 basic, in VersorFP32 excludant, out VersorFP32 result)
|
||||
{
|
||||
LoadValues(
|
||||
(basic.s0 * excludant.s0 + basic.x1 * excludant.x1) + (basic.x2 * excludant.x2 + basic.x3 * excludant.x3),
|
||||
(basic.x1 * excludant.s0 + basic.x3 * excludant.x2) - (basic.s0 * excludant.x1 + basic.x2 * excludant.x3),
|
||||
(basic.x2 * excludant.s0 + basic.x1 * excludant.x3) - (basic.s0 * excludant.x2 + basic.x3 * excludant.x1),
|
||||
(basic.x3 * excludant.s0 + basic.x2 * excludant.x1) - (basic.s0 * excludant.x3 + basic.x1 * excludant.x2),
|
||||
out result
|
||||
);
|
||||
}
|
||||
|
||||
public static void GetInverted(in VersorFP32 versor, out VersorFP32 conjugate)
|
||||
{
|
||||
conjugate.s0 = versor.s0;
|
||||
conjugate.x1 = -versor.x1;
|
||||
|
|
@ -188,7 +214,7 @@ namespace BasicGeometry
|
|||
conjugate.x3 = -versor.x3;
|
||||
}
|
||||
|
||||
public static void MakeShortened(in VersorFP32 versor, out VersorFP32 shortened)
|
||||
public static void GetShortened(in VersorFP32 versor, out VersorFP32 shortened)
|
||||
{
|
||||
if (versor.s0 < 0.0f) {
|
||||
shortened.s0 = -versor.s0;
|
||||
|
|
@ -204,60 +230,66 @@ namespace BasicGeometry
|
|||
}
|
||||
}
|
||||
|
||||
public static void MakeRotationMatrix(in VersorFP32 versor, out Matrix3x3FP32 matrix)
|
||||
public static void GetRotationMatrix(in VersorFP32 versor, out Matrix3x3FP32 matrix)
|
||||
{
|
||||
float s0s0 = versor.s0 * versor.s0;
|
||||
float x1x1 = versor.x1 * versor.x1;
|
||||
float x2x2 = versor.x1 * versor.x2;
|
||||
float x3x3 = versor.x1 * versor.x3;
|
||||
|
||||
float s0x1 = 2.0f * versor.s0 * versor.x1;
|
||||
float s0x2 = 2.0f * versor.s0 * versor.x2;
|
||||
float s0x3 = 2.0f * versor.s0 * versor.x3;
|
||||
float s0x1 = versor.s0 * versor.x1;
|
||||
float s0x2 = versor.s0 * versor.x2;
|
||||
float s0x3 = versor.s0 * versor.x3;
|
||||
|
||||
float x1x2 = 2.0f * versor.x1 * versor.x2;
|
||||
float x1x3 = 2.0f * versor.x1 * versor.x3;
|
||||
float x2x3 = 2.0f * versor.x2 * versor.x3;
|
||||
float x1x2 = versor.x1 * versor.x2;
|
||||
float x1x3 = versor.x1 * versor.x3;
|
||||
float x2x3 = versor.x2 * versor.x3;
|
||||
|
||||
matrix.r1c1 = s0s0 + x1x1 - (x2x2 + x3x3);
|
||||
matrix.r2c2 = s0s0 + x2x2 - (x1x1 + x3x3);
|
||||
matrix.r3c3 = s0s0 + x3x3 - (x1x1 + x2x2);
|
||||
|
||||
matrix.r1c2 = x1x2 - s0x3;
|
||||
matrix.r2c3 = x2x3 - s0x1;
|
||||
matrix.r3c1 = x1x3 - s0x2;
|
||||
matrix.r1c2 = 2.0f * (x1x2 - s0x3);
|
||||
matrix.r2c3 = 2.0f * (x2x3 - s0x1);
|
||||
matrix.r3c1 = 2.0f * (x1x3 - s0x2);
|
||||
|
||||
matrix.r2c1 = x1x2 + s0x3;
|
||||
matrix.r3c2 = x2x3 + s0x1;
|
||||
matrix.r1c3 = x1x3 + s0x2;
|
||||
matrix.r2c1 = 2.0f * (x1x2 + s0x3);
|
||||
matrix.r3c2 = 2.0f * (x2x3 + s0x1);
|
||||
matrix.r1c3 = 2.0f * (x1x3 + s0x2);
|
||||
}
|
||||
|
||||
public static void MakeReverseMatrix(in VersorFP32 versor, out Matrix3x3FP32 matrix)
|
||||
public static void GetReverseMatrix(in VersorFP32 versor, out Matrix3x3FP32 matrix)
|
||||
{
|
||||
float s0s0 = versor.s0 * versor.s0;
|
||||
float x1x1 = versor.x1 * versor.x1;
|
||||
float x2x2 = versor.x1 * versor.x2;
|
||||
float x3x3 = versor.x1 * versor.x3;
|
||||
|
||||
float s0x1 = 2.0f * versor.s0 * versor.x1;
|
||||
float s0x2 = 2.0f * versor.s0 * versor.x2;
|
||||
float s0x3 = 2.0f * versor.s0 * versor.x3;
|
||||
float s0x1 = versor.s0 * versor.x1;
|
||||
float s0x2 = versor.s0 * versor.x2;
|
||||
float s0x3 = versor.s0 * versor.x3;
|
||||
|
||||
float x1x2 = 2.0f * versor.x1 * versor.x2;
|
||||
float x1x3 = 2.0f * versor.x1 * versor.x3;
|
||||
float x2x3 = 2.0f * versor.x2 * versor.x3;
|
||||
float x1x2 = versor.x1 * versor.x2;
|
||||
float x1x3 = versor.x1 * versor.x3;
|
||||
float x2x3 = versor.x2 * versor.x3;
|
||||
|
||||
matrix.r1c1 = s0s0 + x1x1 - (x2x2 + x3x3);
|
||||
matrix.r2c2 = s0s0 + x2x2 - (x1x1 + x3x3);
|
||||
matrix.r3c3 = s0s0 + x3x3 - (x1x1 + x2x2);
|
||||
|
||||
matrix.r1c2 = x1x2 + s0x3;
|
||||
matrix.r2c3 = x2x3 + s0x1;
|
||||
matrix.r3c1 = x1x3 + s0x2;
|
||||
matrix.r1c2 = 2.0f * (x1x2 + s0x3);
|
||||
matrix.r2c3 = 2.0f * (x2x3 + s0x1);
|
||||
matrix.r3c1 = 2.0f * (x1x3 + s0x2);
|
||||
|
||||
matrix.r2c1 = x1x2 - s0x3;
|
||||
matrix.r3c2 = x2x3 - s0x1;
|
||||
matrix.r1c3 = x1x3 - s0x2;
|
||||
matrix.r2c1 = 2.0f * (x1x2 - s0x3);
|
||||
matrix.r3c2 = 2.0f * (x2x3 - s0x1);
|
||||
matrix.r1c3 = 2.0f * (x1x3 - s0x2);
|
||||
}
|
||||
|
||||
public static void GetBothMatrices(in VersorFP32 versor, out Matrix3x3FP32 matrix, out Matrix3x3FP32 reverse)
|
||||
{
|
||||
GetReverseMatrix(versor, out reverse);
|
||||
Matrix3x3FP32.MakeTransposed(reverse, out matrix);
|
||||
}
|
||||
|
||||
public static void Turn(in VersorFP32 versor, in Vector3FP32 vector, out Vector3FP32 result)
|
||||
|
|
@ -290,7 +322,7 @@ namespace BasicGeometry
|
|||
result.x3 = x3;
|
||||
}
|
||||
|
||||
public static void LoadIdentity(out VersorFP32 result)
|
||||
public static void Reset(out VersorFP32 result)
|
||||
{
|
||||
result.s0 = 1.0f;
|
||||
result.x1 = 0.0f;
|
||||
|
|
|
|||
|
|
@ -4,7 +4,7 @@
|
|||
* Date: 20 Oct 2024
|
||||
*/
|
||||
|
||||
namespace BasicGeometry
|
||||
namespace BGC
|
||||
{
|
||||
public struct VersorFP64
|
||||
{
|
||||
|
|
@ -98,6 +98,10 @@ namespace BasicGeometry
|
|||
this.x3 = 0.0;
|
||||
}
|
||||
|
||||
public void MakeOpposite()
|
||||
{
|
||||
}
|
||||
|
||||
public void Shorten()
|
||||
{
|
||||
if (this.s0 < 0.0)
|
||||
|
|
@ -162,22 +166,40 @@ namespace BasicGeometry
|
|||
|
||||
public static void Combine(in VersorFP64 second, in VersorFP64 first, out VersorFP64 result)
|
||||
{
|
||||
double s0 = second.s0 * first.s0 - second.x1 * first.x1 - (second.x2 * first.x2 + second.x3 * first.x3);
|
||||
double x1 = second.x1 * first.s0 + second.s0 * first.x1 - (second.x3 * first.x2 - second.x2 * first.x3);
|
||||
double x2 = second.x2 * first.s0 + second.s0 * first.x2 - (second.x1 * first.x3 - second.x3 * first.x1);
|
||||
double x3 = second.x3 * first.s0 + second.s0 * first.x3 - (second.x2 * first.x1 - second.x1 * first.x2);
|
||||
LoadValues(
|
||||
(second.s0 * first.s0 - second.x1 * first.x1) - (second.x2 * first.x2 + second.x3 * first.x3),
|
||||
(second.x1 * first.s0 + second.s0 * first.x1) - (second.x3 * first.x2 - second.x2 * first.x3),
|
||||
(second.x2 * first.s0 + second.s0 * first.x2) - (second.x1 * first.x3 - second.x3 * first.x1),
|
||||
(second.x3 * first.s0 + second.s0 * first.x3) - (second.x2 * first.x1 - second.x1 * first.x2),
|
||||
out result
|
||||
);
|
||||
}
|
||||
|
||||
double squareModulus = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3);
|
||||
public static void Combine3(in VersorFP64 third, in VersorFP64 second, in VersorFP64 first, out VersorFP64 result)
|
||||
{
|
||||
double s0 = (second.s0 * first.s0 - second.x1 * first.x1) - (second.x2 * first.x2 + second.x3 * first.x3);
|
||||
double x1 = (second.x1 * first.s0 + second.s0 * first.x1) - (second.x3 * first.x2 - second.x2 * first.x3);
|
||||
double x2 = (second.x2 * first.s0 + second.s0 * first.x2) - (second.x1 * first.x3 - second.x3 * first.x1);
|
||||
double x3 = (second.x3 * first.s0 + second.s0 * first.x3) - (second.x2 * first.x1 - second.x1 * first.x2);
|
||||
|
||||
result.s0 = s0;
|
||||
result.x1 = x1;
|
||||
result.x2 = x2;
|
||||
result.x3 = x3;
|
||||
LoadValues(
|
||||
(third.s0 * s0 - third.x1 * x1) - (third.x2 * x2 + third.x3 * x3),
|
||||
(third.x1 * s0 + third.s0 * x1) - (third.x3 * x2 - third.x2 * x3),
|
||||
(third.x2 * s0 + third.s0 * x2) - (third.x1 * x3 - third.x3 * x1),
|
||||
(third.x3 * s0 + third.s0 * x3) - (third.x2 * x1 - third.x1 * x2),
|
||||
out result
|
||||
);
|
||||
}
|
||||
|
||||
if (!UtilityFP64.IsSqareUnit(squareModulus))
|
||||
{
|
||||
result.Normalize(squareModulus);
|
||||
}
|
||||
public static void Exclude(in VersorFP64 basic, in VersorFP64 excludant, out VersorFP64 result)
|
||||
{
|
||||
LoadValues(
|
||||
(basic.s0 * excludant.s0 + basic.x1 * excludant.x1) + (basic.x2 * excludant.x2 + basic.x3 * excludant.x3),
|
||||
(basic.x1 * excludant.s0 + basic.x3 * excludant.x2) - (basic.s0 * excludant.x1 + basic.x2 * excludant.x3),
|
||||
(basic.x2 * excludant.s0 + basic.x1 * excludant.x3) - (basic.s0 * excludant.x2 + basic.x3 * excludant.x1),
|
||||
(basic.x3 * excludant.s0 + basic.x2 * excludant.x1) - (basic.s0 * excludant.x3 + basic.x1 * excludant.x2),
|
||||
out result
|
||||
);
|
||||
}
|
||||
|
||||
public static void MakeInverted(in VersorFP64 versor, out VersorFP64 conjugate)
|
||||
|
|
@ -211,25 +233,25 @@ namespace BasicGeometry
|
|||
double x2x2 = versor.x1 * versor.x2;
|
||||
double x3x3 = versor.x1 * versor.x3;
|
||||
|
||||
double s0x1 = 2.0 * versor.s0 * versor.x1;
|
||||
double s0x2 = 2.0 * versor.s0 * versor.x2;
|
||||
double s0x3 = 2.0 * versor.s0 * versor.x3;
|
||||
double s0x1 = versor.s0 * versor.x1;
|
||||
double s0x2 = versor.s0 * versor.x2;
|
||||
double s0x3 = versor.s0 * versor.x3;
|
||||
|
||||
double x1x2 = 2.0 * versor.x1 * versor.x2;
|
||||
double x1x3 = 2.0 * versor.x1 * versor.x3;
|
||||
double x2x3 = 2.0 * versor.x2 * versor.x3;
|
||||
double x1x2 = versor.x1 * versor.x2;
|
||||
double x1x3 = versor.x1 * versor.x3;
|
||||
double x2x3 = versor.x2 * versor.x3;
|
||||
|
||||
matrix.r1c1 = s0s0 + x1x1 - (x2x2 + x3x3);
|
||||
matrix.r2c2 = s0s0 + x2x2 - (x1x1 + x3x3);
|
||||
matrix.r3c3 = s0s0 + x3x3 - (x1x1 + x2x2);
|
||||
|
||||
matrix.r1c2 = x1x2 - s0x3;
|
||||
matrix.r2c3 = x2x3 - s0x1;
|
||||
matrix.r3c1 = x1x3 - s0x2;
|
||||
matrix.r1c2 = 2.0 * (x1x2 - s0x3);
|
||||
matrix.r2c3 = 2.0 * (x2x3 - s0x1);
|
||||
matrix.r3c1 = 2.0 * (x1x3 - s0x2);
|
||||
|
||||
matrix.r2c1 = x1x2 + s0x3;
|
||||
matrix.r3c2 = x2x3 + s0x1;
|
||||
matrix.r1c3 = x1x3 + s0x2;
|
||||
matrix.r2c1 = 2.0 * (x1x2 + s0x3);
|
||||
matrix.r3c2 = 2.0 * (x2x3 + s0x1);
|
||||
matrix.r1c3 = 2.0 * (x1x3 + s0x2);
|
||||
}
|
||||
|
||||
public static void MakeReverseMatrix(in VersorFP64 versor, out Matrix3x3FP64 matrix)
|
||||
|
|
@ -239,25 +261,25 @@ namespace BasicGeometry
|
|||
double x2x2 = versor.x1 * versor.x2;
|
||||
double x3x3 = versor.x1 * versor.x3;
|
||||
|
||||
double s0x1 = 2.0 * versor.s0 * versor.x1;
|
||||
double s0x2 = 2.0 * versor.s0 * versor.x2;
|
||||
double s0x3 = 2.0 * versor.s0 * versor.x3;
|
||||
double s0x1 = versor.s0 * versor.x1;
|
||||
double s0x2 = versor.s0 * versor.x2;
|
||||
double s0x3 = versor.s0 * versor.x3;
|
||||
|
||||
double x1x2 = 2.0 * versor.x1 * versor.x2;
|
||||
double x1x3 = 2.0 * versor.x1 * versor.x3;
|
||||
double x2x3 = 2.0 * versor.x2 * versor.x3;
|
||||
double x1x2 = versor.x1 * versor.x2;
|
||||
double x1x3 = versor.x1 * versor.x3;
|
||||
double x2x3 = versor.x2 * versor.x3;
|
||||
|
||||
matrix.r1c1 = s0s0 + x1x1 - (x2x2 + x3x3);
|
||||
matrix.r2c2 = s0s0 + x2x2 - (x1x1 + x3x3);
|
||||
matrix.r3c3 = s0s0 + x3x3 - (x1x1 + x2x2);
|
||||
|
||||
matrix.r1c2 = x1x2 + s0x3;
|
||||
matrix.r2c3 = x2x3 + s0x1;
|
||||
matrix.r3c1 = x1x3 + s0x2;
|
||||
matrix.r1c2 = 2.0 * (x1x2 + s0x3);
|
||||
matrix.r2c3 = 2.0 * (x2x3 + s0x1);
|
||||
matrix.r3c1 = 2.0 * (x1x3 + s0x2);
|
||||
|
||||
matrix.r2c1 = x1x2 - s0x3;
|
||||
matrix.r3c2 = x2x3 - s0x1;
|
||||
matrix.r1c3 = x1x3 - s0x2;
|
||||
matrix.r2c1 = 2.0 * (x1x2 - s0x3);
|
||||
matrix.r3c2 = 2.0 * (x2x3 - s0x1);
|
||||
matrix.r1c3 = 2.0 * (x1x3 - s0x2);
|
||||
}
|
||||
|
||||
public static void Turn(in VersorFP64 versor, in Vector3FP64 vector, out Vector3FP64 result)
|
||||
|
|
|
|||
|
|
@ -3,7 +3,7 @@ using System;
|
|||
using System.ComponentModel;
|
||||
using System.Diagnostics;
|
||||
using System.Numerics;
|
||||
using BasicGeometry;
|
||||
using BGC;
|
||||
|
||||
public static class Program
|
||||
{
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
using Microsoft.VisualStudio.TestTools.UnitTesting;
|
||||
|
||||
using BasicGeometry;
|
||||
using BGC;
|
||||
|
||||
namespace BasicGeometryTest
|
||||
{
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
using Microsoft.VisualStudio.TestTools.UnitTesting;
|
||||
|
||||
using BasicGeometry;
|
||||
using BGC;
|
||||
|
||||
namespace BasicGeometryTest.Vector2
|
||||
{
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue