Реорганизация проекта (в процессе)
This commit is contained in:
commit
1695d9efd2
15 changed files with 687 additions and 652 deletions
|
|
@ -59,8 +59,7 @@ namespace BasicGeometry
|
|||
|
||||
public readonly bool IsSingular()
|
||||
{
|
||||
float determinant = this.GetDeterminant();
|
||||
return -UtilityFP32.EPSYLON <= determinant && determinant <= UtilityFP32.EPSYLON;
|
||||
return UtilityFP32.IsZero(this.GetDeterminant());
|
||||
}
|
||||
|
||||
public void Transpose()
|
||||
|
|
@ -72,7 +71,7 @@ namespace BasicGeometry
|
|||
{
|
||||
float determinant = this.GetDeterminant();
|
||||
|
||||
if (-UtilityFP32.EPSYLON <= determinant && determinant <= UtilityFP32.EPSYLON)
|
||||
if (UtilityFP32.IsZero(determinant))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
|
@ -121,7 +120,7 @@ namespace BasicGeometry
|
|||
this.r2c2 = d2;
|
||||
}
|
||||
|
||||
public void SetValues(in Matrix2x2FP32 matrix)
|
||||
public void Set(in Matrix2x2FP32 matrix)
|
||||
{
|
||||
this.r1c1 = matrix.r1c1;
|
||||
this.r1c2 = matrix.r1c2;
|
||||
|
|
@ -130,7 +129,7 @@ namespace BasicGeometry
|
|||
this.r2c2 = matrix.r2c2;
|
||||
}
|
||||
|
||||
public void SetValues(in Matrix2x2FP64 matrix)
|
||||
public void Set(in Matrix2x2FP64 matrix)
|
||||
{
|
||||
this.r1c1 = (float)matrix.r1c1;
|
||||
this.r1c2 = (float)matrix.r1c2;
|
||||
|
|
|
|||
|
|
@ -59,8 +59,7 @@ namespace BasicGeometry
|
|||
|
||||
public readonly bool IsSingular()
|
||||
{
|
||||
double determinant = this.GetDeterminant();
|
||||
return -UtilityFP64.EPSYLON <= determinant && determinant <= UtilityFP64.EPSYLON;
|
||||
return UtilityFP64.IsZero(this.GetDeterminant());
|
||||
}
|
||||
|
||||
public void Transpose()
|
||||
|
|
@ -72,7 +71,7 @@ namespace BasicGeometry
|
|||
{
|
||||
double determinant = this.GetDeterminant();
|
||||
|
||||
if (-UtilityFP64.EPSYLON <= determinant && determinant <= UtilityFP64.EPSYLON)
|
||||
if (UtilityFP64.IsZero(determinant))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
|
@ -121,7 +120,7 @@ namespace BasicGeometry
|
|||
this.r2c2 = d2;
|
||||
}
|
||||
|
||||
public void SetValues(in Matrix2x2FP64 matrix)
|
||||
public void Set(in Matrix2x2FP64 matrix)
|
||||
{
|
||||
this.r1c1 = matrix.r1c1;
|
||||
this.r1c2 = matrix.r1c2;
|
||||
|
|
@ -130,7 +129,7 @@ namespace BasicGeometry
|
|||
this.r2c2 = matrix.r2c2;
|
||||
}
|
||||
|
||||
public void SetValues(in Matrix2x2FP32 matrix)
|
||||
public void Set(in Matrix2x2FP32 matrix)
|
||||
{
|
||||
this.r1c1 = matrix.r1c1;
|
||||
this.r1c2 = matrix.r1c2;
|
||||
|
|
@ -159,7 +158,7 @@ namespace BasicGeometry
|
|||
{
|
||||
double determinant = matrix.GetDeterminant();
|
||||
|
||||
if (-UtilityFP64.EPSYLON <= determinant && determinant <= UtilityFP64.EPSYLON)
|
||||
if (UtilityFP64.IsZero(determinant))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -75,8 +75,7 @@ namespace BasicGeometry
|
|||
|
||||
public readonly bool IsSingular()
|
||||
{
|
||||
float determinant = this.GetDeterminant();
|
||||
return -UtilityFP32.EPSYLON <= determinant && determinant <= UtilityFP32.EPSYLON;
|
||||
return UtilityFP32.IsZero(this.GetDeterminant());
|
||||
}
|
||||
|
||||
public void Transpose()
|
||||
|
|
@ -90,7 +89,8 @@ namespace BasicGeometry
|
|||
{
|
||||
float determinant = this.GetDeterminant();
|
||||
|
||||
if (-UtilityFP32.EPSYLON <= determinant && determinant <= UtilityFP32.EPSYLON) {
|
||||
if (UtilityFP32.IsZero(determinant))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
|
|
@ -168,7 +168,7 @@ namespace BasicGeometry
|
|||
this.r2c3 = d3;
|
||||
}
|
||||
|
||||
public void SetValues(in Matrix3x3FP32 matrix)
|
||||
public void Set(in Matrix3x3FP32 matrix)
|
||||
{
|
||||
this.r1c1 = matrix.r1c1;
|
||||
this.r1c2 = matrix.r1c2;
|
||||
|
|
@ -183,7 +183,7 @@ namespace BasicGeometry
|
|||
this.r3c3 = matrix.r3c3;
|
||||
}
|
||||
|
||||
public void SetValues(in Matrix3x3FP64 matrix)
|
||||
public void Set(in Matrix3x3FP64 matrix)
|
||||
{
|
||||
this.r1c1 = (float)matrix.r1c1;
|
||||
this.r1c2 = (float)matrix.r1c2;
|
||||
|
|
@ -198,69 +198,6 @@ namespace BasicGeometry
|
|||
this.r3c3 = (float)matrix.r3c3;
|
||||
}
|
||||
|
||||
public void SetTransposedOf(in Matrix3x3FP32 matrix)
|
||||
{
|
||||
this.r1c1 = matrix.r1c1;
|
||||
this.r2c2 = matrix.r2c2;
|
||||
this.r3c3 = matrix.r3c3;
|
||||
|
||||
(this.r1c2, this.r2c1) = (matrix.r2c1, matrix.r1c2);
|
||||
(this.r1c3, this.r3c1) = (matrix.r3c1, matrix.r1c3);
|
||||
(this.r2c3, this.r3c2) = (matrix.r3c2, matrix.r2c3);
|
||||
}
|
||||
|
||||
public void SetTransposedOf(in Matrix3x3FP64 matrix)
|
||||
{
|
||||
this.r1c1 = (float)matrix.r1c1;
|
||||
this.r1c2 = (float)matrix.r2c1;
|
||||
this.r1c3 = (float)matrix.r3c1;
|
||||
|
||||
this.r2c1 = (float)matrix.r1c2;
|
||||
this.r2c2 = (float)matrix.r2c2;
|
||||
this.r2c3 = (float)matrix.r3c2;
|
||||
|
||||
this.r3c1 = (float)matrix.r1c3;
|
||||
this.r3c2 = (float)matrix.r2c3;
|
||||
this.r3c3 = (float)matrix.r3c3;
|
||||
}
|
||||
|
||||
public bool SetInvertedOf(in Matrix3x3FP32 matrix)
|
||||
{
|
||||
float determinant = matrix.GetDeterminant();
|
||||
|
||||
if (-UtilityFP32.EPSYLON <= determinant && determinant <= UtilityFP32.EPSYLON) {
|
||||
return false;
|
||||
}
|
||||
|
||||
float r1c1 = matrix.r2c2 * matrix.r3c3 - matrix.r2c3 * matrix.r3c2;
|
||||
float r1c2 = matrix.r1c3 * matrix.r3c2 - matrix.r1c2 * matrix.r3c3;
|
||||
float r1c3 = matrix.r1c2 * matrix.r2c3 - matrix.r1c3 * matrix.r2c2;
|
||||
|
||||
float r2c1 = matrix.r2c3 * matrix.r3c1 - matrix.r2c1 * matrix.r3c3;
|
||||
float r2c2 = matrix.r1c1 * matrix.r3c3 - matrix.r1c3 * matrix.r3c1;
|
||||
float r2c3 = matrix.r1c3 * matrix.r2c1 - matrix.r1c1 * matrix.r2c3;
|
||||
|
||||
float r3c1 = matrix.r2c1 * matrix.r3c2 - matrix.r2c2 * matrix.r3c1;
|
||||
float r3c2 = matrix.r1c2 * matrix.r3c1 - matrix.r1c1 * matrix.r3c2;
|
||||
float r3c3 = matrix.r1c1 * matrix.r2c2 - matrix.r1c2 * matrix.r2c1;
|
||||
|
||||
float mutiplier = 1.0f / determinant;
|
||||
|
||||
this.r1c1 = r1c1 * mutiplier;
|
||||
this.r1c2 = r1c2 * mutiplier;
|
||||
this.r1c3 = r1c3 * mutiplier;
|
||||
|
||||
this.r2c1 = r2c1 * mutiplier;
|
||||
this.r2c2 = r2c2 * mutiplier;
|
||||
this.r2c3 = r2c3 * mutiplier;
|
||||
|
||||
this.r3c1 = r3c1 * mutiplier;
|
||||
this.r3c2 = r3c2 * mutiplier;
|
||||
this.r3c3 = r3c3 * mutiplier;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
public void SetRow1(float c1, float c2, float c3)
|
||||
{
|
||||
this.r1c1 = c1;
|
||||
|
|
@ -303,19 +240,53 @@ namespace BasicGeometry
|
|||
this.r3c3 = r3;
|
||||
}
|
||||
|
||||
public void AppendScaled(in Matrix3x3FP32 matrix, float scale)
|
||||
public static void MakeTransposed(in Matrix3x3FP64 matrix, out Matrix3x3FP64 transposed)
|
||||
{
|
||||
this.r1c1 += matrix.r1c1* scale;
|
||||
this.r1c2 += matrix.r1c2* scale;
|
||||
this.r1c3 += matrix.r1c3* scale;
|
||||
transposed.r1c1 = matrix.r1c1;
|
||||
transposed.r2c2 = matrix.r2c2;
|
||||
transposed.r3c3 = matrix.r3c3;
|
||||
|
||||
this.r2c1 += matrix.r2c1* scale;
|
||||
this.r2c2 += matrix.r2c2* scale;
|
||||
this.r2c3 += matrix.r2c3* scale;
|
||||
(transposed.r1c2, transposed.r2c1) = (matrix.r2c1, matrix.r1c2);
|
||||
(transposed.r1c3, transposed.r3c1) = (matrix.r3c1, matrix.r1c3);
|
||||
(transposed.r2c3, transposed.r3c2) = (matrix.r3c2, matrix.r2c3);
|
||||
}
|
||||
|
||||
this.r3c1 += matrix.r3c1* scale;
|
||||
this.r3c2 += matrix.r3c2* scale;
|
||||
this.r3c3 += matrix.r3c3* scale;
|
||||
public static bool MakeInverted(in Matrix3x3FP32 matrix, out Matrix3x3FP32 inverted)
|
||||
{
|
||||
float determinant = matrix.GetDeterminant();
|
||||
|
||||
if (UtilityFP32.IsZero(determinant)) {
|
||||
LoadZero(out inverted);
|
||||
return false;
|
||||
}
|
||||
|
||||
float r1c1 = matrix.r2c2 * matrix.r3c3 - matrix.r2c3 * matrix.r3c2;
|
||||
float r1c2 = matrix.r1c3 * matrix.r3c2 - matrix.r1c2 * matrix.r3c3;
|
||||
float r1c3 = matrix.r1c2 * matrix.r2c3 - matrix.r1c3 * matrix.r2c2;
|
||||
|
||||
float r2c1 = matrix.r2c3 * matrix.r3c1 - matrix.r2c1 * matrix.r3c3;
|
||||
float r2c2 = matrix.r1c1 * matrix.r3c3 - matrix.r1c3 * matrix.r3c1;
|
||||
float r2c3 = matrix.r1c3 * matrix.r2c1 - matrix.r1c1 * matrix.r2c3;
|
||||
|
||||
float r3c1 = matrix.r2c1 * matrix.r3c2 - matrix.r2c2 * matrix.r3c1;
|
||||
float r3c2 = matrix.r1c2 * matrix.r3c1 - matrix.r1c1 * matrix.r3c2;
|
||||
float r3c3 = matrix.r1c1 * matrix.r2c2 - matrix.r1c2 * matrix.r2c1;
|
||||
|
||||
float mutiplier = 1.0f / determinant;
|
||||
|
||||
inverted.r1c1 = r1c1 * mutiplier;
|
||||
inverted.r1c2 = r1c2 * mutiplier;
|
||||
inverted.r1c3 = r1c3 * mutiplier;
|
||||
|
||||
inverted.r2c1 = r2c1 * mutiplier;
|
||||
inverted.r2c2 = r2c2 * mutiplier;
|
||||
inverted.r2c3 = r2c3 * mutiplier;
|
||||
|
||||
inverted.r3c1 = r3c1 * mutiplier;
|
||||
inverted.r3c2 = r3c2 * mutiplier;
|
||||
inverted.r3c3 = r3c3 * mutiplier;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
public static void Add(in Matrix3x3FP32 matrix1, in Matrix3x3FP32 matrix2, out Matrix3x3FP32 sum)
|
||||
|
|
@ -333,6 +304,21 @@ namespace BasicGeometry
|
|||
sum.r3c3 = matrix1.r3c3 + matrix2.r3c3;
|
||||
}
|
||||
|
||||
public static void AddScaled(in Matrix3x3FP32 basicMatrix, in Matrix3x3FP32 scalableMatrix, float scale, out Matrix3x3FP32 sum)
|
||||
{
|
||||
sum.r1c1 = basicMatrix.r1c1 + scalableMatrix.r1c1 * scale;
|
||||
sum.r1c2 = basicMatrix.r1c2 + scalableMatrix.r1c2 * scale;
|
||||
sum.r1c3 = basicMatrix.r1c3 + scalableMatrix.r1c3 * scale;
|
||||
|
||||
sum.r2c1 = basicMatrix.r2c1 + scalableMatrix.r2c1 * scale;
|
||||
sum.r2c2 = basicMatrix.r2c2 + scalableMatrix.r2c2 * scale;
|
||||
sum.r2c3 = basicMatrix.r2c3 + scalableMatrix.r2c3 * scale;
|
||||
|
||||
sum.r3c1 = basicMatrix.r3c1 + scalableMatrix.r3c1 * scale;
|
||||
sum.r3c2 = basicMatrix.r3c2 + scalableMatrix.r3c2 * scale;
|
||||
sum.r3c3 = basicMatrix.r3c3 + scalableMatrix.r3c3 * scale;
|
||||
}
|
||||
|
||||
public static void Subtract(in Matrix3x3FP32 minuend, in Matrix3x3FP32 subtrahend, out Matrix3x3FP32 difference)
|
||||
{
|
||||
difference.r1c1 = minuend.r1c1 - subtrahend.r1c1;
|
||||
|
|
@ -348,6 +334,21 @@ 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;
|
||||
|
|
@ -368,7 +369,7 @@ namespace BasicGeometry
|
|||
Multiply(dividend, 1.0f / divisor, out quotient);
|
||||
}
|
||||
|
||||
public static void GetRightProduct(in Matrix3x3FP32 matrix, in Vector3FP32 vector, out Vector3FP32 result)
|
||||
public static void RightProduct(in Matrix3x3FP32 matrix, in Vector3FP32 vector, out Vector3FP32 result)
|
||||
{
|
||||
float x1 = matrix.r1c1 * vector.x1 + matrix.r1c2 * vector.x2 + matrix.r1c3 * vector.x3;
|
||||
float x2 = matrix.r2c1 * vector.x1 + matrix.r2c2 * vector.x2 + matrix.r2c3 * vector.x3;
|
||||
|
|
@ -379,7 +380,7 @@ namespace BasicGeometry
|
|||
result.x3 = x3;
|
||||
}
|
||||
|
||||
public static void GetLeftProduct(in Vector3FP32 vector, in Matrix3x3FP32 matrix, out Vector3FP32 result)
|
||||
public static void LeftProduct(in Vector3FP32 vector, in Matrix3x3FP32 matrix, out Vector3FP32 result)
|
||||
{
|
||||
float x1 = vector.x1 * matrix.r1c1 + vector.x2 * matrix.r2c1 + vector.x3 * matrix.r3c1;
|
||||
float x2 = vector.x1 * matrix.r1c2 + vector.x2 * matrix.r2c2 + vector.x3 * matrix.r3c2;
|
||||
|
|
|
|||
|
|
@ -75,8 +75,7 @@ namespace BasicGeometry
|
|||
|
||||
public readonly bool IsSingular()
|
||||
{
|
||||
double determinant = this.GetDeterminant();
|
||||
return -UtilityFP64.EPSYLON <= determinant && determinant <= UtilityFP64.EPSYLON;
|
||||
return UtilityFP64.IsZero(this.GetDeterminant());
|
||||
}
|
||||
|
||||
public void Transpose()
|
||||
|
|
@ -90,7 +89,8 @@ namespace BasicGeometry
|
|||
{
|
||||
double determinant = this.GetDeterminant();
|
||||
|
||||
if (-UtilityFP64.EPSYLON <= determinant && determinant <= UtilityFP64.EPSYLON) {
|
||||
if (UtilityFP64.IsZero(determinant))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
|
|
@ -168,7 +168,7 @@ namespace BasicGeometry
|
|||
this.r2c3 = d3;
|
||||
}
|
||||
|
||||
public void SetValues(Matrix3x3FP64 matrix)
|
||||
public void Set(Matrix3x3FP64 matrix)
|
||||
{
|
||||
this.r1c1 = matrix.r1c1;
|
||||
this.r1c2 = matrix.r1c2;
|
||||
|
|
@ -183,7 +183,7 @@ namespace BasicGeometry
|
|||
this.r3c3 = matrix.r3c3;
|
||||
}
|
||||
|
||||
public void SetValues(Matrix3x3FP32 matrix)
|
||||
public void Set(Matrix3x3FP32 matrix)
|
||||
{
|
||||
this.r1c1 = matrix.r1c1;
|
||||
this.r1c2 = matrix.r1c2;
|
||||
|
|
@ -198,65 +198,6 @@ namespace BasicGeometry
|
|||
this.r3c3 = matrix.r3c3;
|
||||
}
|
||||
|
||||
public void SetTransposedOf(in Matrix3x3FP64 matrix)
|
||||
{
|
||||
this.r1c1 = matrix.r1c1;
|
||||
this.r2c2 = matrix.r2c2;
|
||||
this.r3c3 = matrix.r3c3;
|
||||
|
||||
(this.r1c2, this.r2c1) = (matrix.r2c1, matrix.r1c2);
|
||||
(this.r1c3, this.r3c1) = (matrix.r3c1, matrix.r1c3);
|
||||
(this.r2c3, this.r3c2) = (matrix.r3c2, matrix.r2c3);
|
||||
}
|
||||
|
||||
public void SetTransposedOf(in Matrix3x3FP32 matrix)
|
||||
{
|
||||
this.r1c1 = matrix.r1c1;
|
||||
this.r2c2 = matrix.r2c2;
|
||||
this.r3c3 = matrix.r3c3;
|
||||
|
||||
(this.r1c2, this.r2c1) = (matrix.r2c1, matrix.r1c2);
|
||||
(this.r1c3, this.r3c1) = (matrix.r3c1, matrix.r1c3);
|
||||
(this.r2c3, this.r3c2) = (matrix.r3c2, matrix.r2c3);
|
||||
}
|
||||
|
||||
public bool SetInvertedOf(in Matrix3x3FP64 matrix)
|
||||
{
|
||||
double determinant = matrix.GetDeterminant();
|
||||
|
||||
if (-UtilityFP64.EPSYLON <= determinant && determinant <= UtilityFP64.EPSYLON) {
|
||||
return false;
|
||||
}
|
||||
|
||||
double r1c1 = matrix.r2c2 * matrix.r3c3 - matrix.r2c3 * matrix.r3c2;
|
||||
double r1c2 = matrix.r1c3 * matrix.r3c2 - matrix.r1c2 * matrix.r3c3;
|
||||
double r1c3 = matrix.r1c2 * matrix.r2c3 - matrix.r1c3 * matrix.r2c2;
|
||||
|
||||
double r2c1 = matrix.r2c3 * matrix.r3c1 - matrix.r2c1 * matrix.r3c3;
|
||||
double r2c2 = matrix.r1c1 * matrix.r3c3 - matrix.r1c3 * matrix.r3c1;
|
||||
double r2c3 = matrix.r1c3 * matrix.r2c1 - matrix.r1c1 * matrix.r2c3;
|
||||
|
||||
double r3c1 = matrix.r2c1 * matrix.r3c2 - matrix.r2c2 * matrix.r3c1;
|
||||
double r3c2 = matrix.r1c2 * matrix.r3c1 - matrix.r1c1 * matrix.r3c2;
|
||||
double r3c3 = matrix.r1c1 * matrix.r2c2 - matrix.r1c2 * matrix.r2c1;
|
||||
|
||||
double mutiplier = 1.0 / determinant;
|
||||
|
||||
this.r1c1 = r1c1 * mutiplier;
|
||||
this.r1c2 = r1c2 * mutiplier;
|
||||
this.r1c3 = r1c3 * mutiplier;
|
||||
|
||||
this.r2c1 = r2c1 * mutiplier;
|
||||
this.r2c2 = r2c2 * mutiplier;
|
||||
this.r2c3 = r2c3 * mutiplier;
|
||||
|
||||
this.r3c1 = r3c1 * mutiplier;
|
||||
this.r3c2 = r3c2 * mutiplier;
|
||||
this.r3c3 = r3c3 * mutiplier;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
public void SetRow1(double c1, double c2, double c3)
|
||||
{
|
||||
this.r1c1 = c1;
|
||||
|
|
@ -299,19 +240,53 @@ namespace BasicGeometry
|
|||
this.r3c3 = r3;
|
||||
}
|
||||
|
||||
public void AppendScaled(in Matrix3x3FP64 matrix, double scale)
|
||||
public static void MakeTransposed(in Matrix3x3FP64 matrix, out Matrix3x3FP64 transposed)
|
||||
{
|
||||
this.r1c1 += matrix.r1c1 * scale;
|
||||
this.r1c2 += matrix.r1c2 * scale;
|
||||
this.r1c3 += matrix.r1c3 * scale;
|
||||
transposed.r1c1 = matrix.r1c1;
|
||||
transposed.r2c2 = matrix.r2c2;
|
||||
transposed.r3c3 = matrix.r3c3;
|
||||
|
||||
this.r2c1 += matrix.r2c1 * scale;
|
||||
this.r2c2 += matrix.r2c2 * scale;
|
||||
this.r2c3 += matrix.r2c3 * scale;
|
||||
(transposed.r1c2, transposed.r2c1) = (matrix.r2c1, matrix.r1c2);
|
||||
(transposed.r1c3, transposed.r3c1) = (matrix.r3c1, matrix.r1c3);
|
||||
(transposed.r2c3, transposed.r3c2) = (matrix.r3c2, matrix.r2c3);
|
||||
}
|
||||
|
||||
this.r3c1 += matrix.r3c1 * scale;
|
||||
this.r3c2 += matrix.r3c2 * scale;
|
||||
this.r3c3 += matrix.r3c3 * scale;
|
||||
public static bool MakeInverted(in Matrix3x3FP64 matrix, out Matrix3x3FP64 inverted)
|
||||
{
|
||||
double determinant = matrix.GetDeterminant();
|
||||
|
||||
if (UtilityFP64.IsZero(determinant)) {
|
||||
LoadZero(out inverted);
|
||||
return false;
|
||||
}
|
||||
|
||||
double r1c1 = matrix.r2c2 * matrix.r3c3 - matrix.r2c3 * matrix.r3c2;
|
||||
double r1c2 = matrix.r1c3 * matrix.r3c2 - matrix.r1c2 * matrix.r3c3;
|
||||
double r1c3 = matrix.r1c2 * matrix.r2c3 - matrix.r1c3 * matrix.r2c2;
|
||||
|
||||
double r2c1 = matrix.r2c3 * matrix.r3c1 - matrix.r2c1 * matrix.r3c3;
|
||||
double r2c2 = matrix.r1c1 * matrix.r3c3 - matrix.r1c3 * matrix.r3c1;
|
||||
double r2c3 = matrix.r1c3 * matrix.r2c1 - matrix.r1c1 * matrix.r2c3;
|
||||
|
||||
double r3c1 = matrix.r2c1 * matrix.r3c2 - matrix.r2c2 * matrix.r3c1;
|
||||
double r3c2 = matrix.r1c2 * matrix.r3c1 - matrix.r1c1 * matrix.r3c2;
|
||||
double r3c3 = matrix.r1c1 * matrix.r2c2 - matrix.r1c2 * matrix.r2c1;
|
||||
|
||||
double mutiplier = 1.0 / determinant;
|
||||
|
||||
inverted.r1c1 = r1c1 * mutiplier;
|
||||
inverted.r1c2 = r1c2 * mutiplier;
|
||||
inverted.r1c3 = r1c3 * mutiplier;
|
||||
|
||||
inverted.r2c1 = r2c1 * mutiplier;
|
||||
inverted.r2c2 = r2c2 * mutiplier;
|
||||
inverted.r2c3 = r2c3 * mutiplier;
|
||||
|
||||
inverted.r3c1 = r3c1 * mutiplier;
|
||||
inverted.r3c2 = r3c2 * mutiplier;
|
||||
inverted.r3c3 = r3c3 * mutiplier;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
public static void Add(in Matrix3x3FP64 matrix1, in Matrix3x3FP64 matrix2, out Matrix3x3FP64 sum)
|
||||
|
|
@ -329,6 +304,21 @@ namespace BasicGeometry
|
|||
sum.r3c3 = matrix1.r3c3 + matrix2.r3c3;
|
||||
}
|
||||
|
||||
public static void AddScaled(in Matrix3x3FP64 basicMatrix, in Matrix3x3FP64 scalableMatrix, double scale, out Matrix3x3FP64 sum)
|
||||
{
|
||||
sum.r1c1 = basicMatrix.r1c1 + scalableMatrix.r1c1 * scale;
|
||||
sum.r1c2 = basicMatrix.r1c2 + scalableMatrix.r1c2 * scale;
|
||||
sum.r1c3 = basicMatrix.r1c3 + scalableMatrix.r1c3 * scale;
|
||||
|
||||
sum.r2c1 = basicMatrix.r2c1 + scalableMatrix.r2c1 * scale;
|
||||
sum.r2c2 = basicMatrix.r2c2 + scalableMatrix.r2c2 * scale;
|
||||
sum.r2c3 = basicMatrix.r2c3 + scalableMatrix.r2c3 * scale;
|
||||
|
||||
sum.r3c1 = basicMatrix.r3c1 + scalableMatrix.r3c1 * scale;
|
||||
sum.r3c2 = basicMatrix.r3c2 + scalableMatrix.r3c2 * scale;
|
||||
sum.r3c3 = basicMatrix.r3c3 + scalableMatrix.r3c3 * scale;
|
||||
}
|
||||
|
||||
public static void Subtract(in Matrix3x3FP64 minuend, in Matrix3x3FP64 subtrahend, out Matrix3x3FP64 difference)
|
||||
{
|
||||
difference.r1c1 = minuend.r1c1 - subtrahend.r1c1;
|
||||
|
|
@ -344,6 +334,21 @@ 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;
|
||||
|
|
@ -364,7 +369,7 @@ namespace BasicGeometry
|
|||
Multiply(dividend, 1.0 / divisor, out quotient);
|
||||
}
|
||||
|
||||
public static void GetRightProduct(in Matrix3x3FP64 matrix, in Vector3FP64 vector, out Vector3FP64 result)
|
||||
public static void RightProduct(in Matrix3x3FP64 matrix, in Vector3FP64 vector, out Vector3FP64 result)
|
||||
{
|
||||
double x1 = matrix.r1c1 * vector.x1 + matrix.r1c2 * vector.x2 + matrix.r1c3 * vector.x3;
|
||||
double x2 = matrix.r2c1 * vector.x1 + matrix.r2c2 * vector.x2 + matrix.r2c3 * vector.x3;
|
||||
|
|
@ -375,7 +380,7 @@ namespace BasicGeometry
|
|||
result.x3 = x3;
|
||||
}
|
||||
|
||||
public static void GetLeftProduct(in Vector3FP64 vector, in Matrix3x3FP64 matrix, out Vector3FP64 result)
|
||||
public static void LeftProduct(in Vector3FP64 vector, in Matrix3x3FP64 matrix, out Vector3FP64 result)
|
||||
{
|
||||
double x1 = vector.x1 * matrix.r1c1 + vector.x2 * matrix.r2c1 + vector.x3 * matrix.r3c1;
|
||||
double x2 = vector.x1 * matrix.r1c2 + vector.x2 * matrix.r2c2 + vector.x3 * matrix.r3c2;
|
||||
|
|
|
|||
|
|
@ -30,24 +30,24 @@ namespace BasicGeometry
|
|||
this.x3 = (float)quaternion.x3;
|
||||
}
|
||||
|
||||
public readonly float GetSquareModule()
|
||||
public readonly float GetSquareModulus()
|
||||
{
|
||||
return this.s0 * this.s0 + this.x1 * this.x1 + (this.x2 * this.x2 + this.x3 * this.x3);
|
||||
}
|
||||
|
||||
public readonly float GetModule()
|
||||
public readonly float GetModulus()
|
||||
{
|
||||
return MathF.Sqrt(this.GetSquareModule());
|
||||
return MathF.Sqrt(this.GetSquareModulus());
|
||||
}
|
||||
|
||||
public readonly bool IsZero()
|
||||
{
|
||||
return this.GetSquareModule() <= UtilityFP32.SQUARE_EPSYLON;
|
||||
return this.GetSquareModulus() <= UtilityFP32.SQUARE_EPSYLON;
|
||||
}
|
||||
|
||||
public readonly bool IsUnit()
|
||||
{
|
||||
return UtilityFP32.IsSqareValueUnit(this.GetSquareModule());
|
||||
return UtilityFP32.IsSqareUnit(this.GetSquareModulus());
|
||||
}
|
||||
|
||||
public void Reset()
|
||||
|
|
@ -81,7 +81,7 @@ namespace BasicGeometry
|
|||
this.x3 = x3;
|
||||
}
|
||||
|
||||
public void SetValues(in QuaternionFP32 quaternion)
|
||||
public void Set(in QuaternionFP32 quaternion)
|
||||
{
|
||||
this.s0 = quaternion.s0;
|
||||
this.x1 = quaternion.x1;
|
||||
|
|
@ -97,12 +97,12 @@ namespace BasicGeometry
|
|||
this.x3 = (float)quaternion.x3;
|
||||
}
|
||||
|
||||
public void SetConjugateOf(in QuaternionFP32 quaternion)
|
||||
public static void MakeConjugate(in QuaternionFP32 quaternion, out QuaternionFP32 conjugate)
|
||||
{
|
||||
this.s0 = quaternion.s0;
|
||||
this.x1 = -quaternion.x1;
|
||||
this.x2 = -quaternion.x2;
|
||||
this.x3 = -quaternion.x3;
|
||||
conjugate.s0 = quaternion.s0;
|
||||
conjugate.x1 = -quaternion.x1;
|
||||
conjugate.x2 = -quaternion.x2;
|
||||
conjugate.x3 = -quaternion.x3;
|
||||
}
|
||||
|
||||
public readonly void MakeRotationMatrix(out Matrix3x3FP32 matrix)
|
||||
|
|
@ -112,16 +112,15 @@ namespace BasicGeometry
|
|||
float x2x2 = this.x2 * this.x2;
|
||||
float x3x3 = this.x3 * this.x3;
|
||||
|
||||
float squareModule = (s0s0 + x1x1) + (x2x2 + x3x3);
|
||||
float squareModulus = (s0s0 + x1x1) + (x2x2 + x3x3);
|
||||
|
||||
if (squareModule <= UtilityFP32.SQUARE_EPSYLON)
|
||||
if (squareModulus <= UtilityFP32.SQUARE_EPSYLON)
|
||||
{
|
||||
Matrix3x3FP32.LoadIdentity(out matrix);
|
||||
return;
|
||||
}
|
||||
|
||||
float corrector1 = 1.0f / squareModule;
|
||||
float corrector2 = 2.0f * corrector1;
|
||||
float corrector1 = 1.0f / squareModulus;
|
||||
|
||||
float s0x1 = this.s0 * this.x1;
|
||||
float s0x2 = this.s0 * this.x2;
|
||||
|
|
@ -130,6 +129,8 @@ namespace BasicGeometry
|
|||
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));
|
||||
|
|
@ -150,16 +151,15 @@ namespace BasicGeometry
|
|||
float x2x2 = this.x2 * this.x2;
|
||||
float x3x3 = this.x3 * this.x3;
|
||||
|
||||
float squareModule = (s0s0 + x1x1) + (x2x2 + x3x3);
|
||||
float squareModulus = (s0s0 + x1x1) + (x2x2 + x3x3);
|
||||
|
||||
if (squareModule <= UtilityFP32.SQUARE_EPSYLON)
|
||||
if (squareModulus <= UtilityFP32.SQUARE_EPSYLON)
|
||||
{
|
||||
Matrix3x3FP32.LoadIdentity(out matrix);
|
||||
return;
|
||||
}
|
||||
|
||||
float corrector1 = 1.0f / squareModule;
|
||||
float corrector2 = 2.0f * corrector1;
|
||||
float corrector1 = 1.0f / squareModulus;
|
||||
|
||||
float s0x1 = this.s0 * this.x1;
|
||||
float s0x2 = this.s0 * this.x2;
|
||||
|
|
@ -168,6 +168,8 @@ namespace BasicGeometry
|
|||
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));
|
||||
|
|
@ -199,15 +201,33 @@ namespace BasicGeometry
|
|||
|
||||
public static void Multiply(in QuaternionFP32 left, in QuaternionFP32 right, out QuaternionFP32 product)
|
||||
{
|
||||
float s0 = (left.s0 * right.s0 - left.x1 * right.x1) - (left.x2 * right.x2 + left.x3 * right.x3);
|
||||
float x1 = (left.x1 * right.s0 + left.s0 * right.x1) - (left.x3 * right.x2 - left.x2 * right.x3);
|
||||
float x2 = (left.x2 * right.s0 + left.s0 * right.x2) - (left.x1 * right.x3 - left.x3 * right.x1);
|
||||
float x3 = (left.x3 * right.s0 + left.s0 * right.x3) - (left.x2 * right.x1 - left.x1 * right.x2);
|
||||
float s0 = left.s0 * right.s0 - left.x1 * right.x1 - (left.x2 * right.x2 + left.x3 * right.x3);
|
||||
float x1 = left.x1 * right.s0 + left.s0 * right.x1 - (left.x3 * right.x2 - left.x2 * right.x3);
|
||||
float x2 = left.x2 * right.s0 + left.s0 * right.x2 - (left.x1 * right.x3 - left.x3 * right.x1);
|
||||
float x3 = left.x3 * right.s0 + left.s0 * right.x3 - (left.x2 * right.x1 - left.x1 * right.x2);
|
||||
|
||||
product.s0 = s0;
|
||||
product.x1 = x1;
|
||||
product.x2 = x2;
|
||||
product.x3 = x3;
|
||||
}
|
||||
|
||||
public static bool AreClose(QuaternionFP32 quaternion1, QuaternionFP32 quaternion2)
|
||||
{
|
||||
float ds0 = quaternion1.s0 - quaternion2.s0;
|
||||
float dx1 = quaternion1.x1 - quaternion2.x1;
|
||||
float dx2 = quaternion1.x2 - quaternion2.x2;
|
||||
float dx3 = quaternion1.x3 - quaternion2.x3;
|
||||
|
||||
float squareModulus1 = quaternion1.GetSquareModulus();
|
||||
float squareModulus2 = quaternion2.GetSquareModulus();
|
||||
float squareDistance = (ds0 * ds0 + dx1 * dx1) + (dx2 * dx2 + dx3 * dx3);
|
||||
|
||||
if (squareModulus1 <= UtilityFP32.EPSYLON_EFFECTIVENESS_LIMIT || squareModulus2 <= UtilityFP32.EPSYLON_EFFECTIVENESS_LIMIT) {
|
||||
return squareDistance <= UtilityFP32.SQUARE_EPSYLON;
|
||||
}
|
||||
|
||||
return squareDistance <= UtilityFP32.SQUARE_EPSYLON * squareModulus1 && squareDistance <= UtilityFP32.SQUARE_EPSYLON * squareModulus2;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,5 +1,4 @@
|
|||
using System;
|
||||
using System.Numerics;
|
||||
|
||||
namespace BasicGeometry
|
||||
{
|
||||
|
|
@ -31,24 +30,24 @@ namespace BasicGeometry
|
|||
this.x3 = quaternion.x3;
|
||||
}
|
||||
|
||||
public readonly double GetSquareModule()
|
||||
public readonly double GetSquareModulus()
|
||||
{
|
||||
return this.s0 * this.s0 + this.x1 * this.x1 + (this.x2 * this.x2 + this.x3 * this.x3);
|
||||
}
|
||||
|
||||
public readonly double GetModule()
|
||||
public readonly double GetModulus()
|
||||
{
|
||||
return Math.Sqrt(this.GetSquareModule());
|
||||
return Math.Sqrt(this.GetSquareModulus());
|
||||
}
|
||||
|
||||
public readonly bool IsZero()
|
||||
{
|
||||
return this.GetSquareModule() <= UtilityFP64.SQUARE_EPSYLON;
|
||||
return this.GetSquareModulus() <= UtilityFP64.SQUARE_EPSYLON;
|
||||
}
|
||||
|
||||
public readonly bool IsUnit()
|
||||
{
|
||||
return UtilityFP64.IsSqareValueUnit(this.GetSquareModule());
|
||||
return UtilityFP64.IsSqareUnit(this.GetSquareModulus());
|
||||
}
|
||||
|
||||
public void Reset()
|
||||
|
|
@ -82,7 +81,7 @@ namespace BasicGeometry
|
|||
this.x3 = x3;
|
||||
}
|
||||
|
||||
public void SetValues(in QuaternionFP32 quaternion)
|
||||
public void Set(in QuaternionFP32 quaternion)
|
||||
{
|
||||
this.s0 = quaternion.s0;
|
||||
this.x1 = quaternion.x1;
|
||||
|
|
@ -90,7 +89,7 @@ namespace BasicGeometry
|
|||
this.x3 = quaternion.x3;
|
||||
}
|
||||
|
||||
public void SetValues(in QuaternionFP64 quaternion)
|
||||
public void Set(in QuaternionFP64 quaternion)
|
||||
{
|
||||
this.s0 = quaternion.s0;
|
||||
this.x1 = quaternion.x1;
|
||||
|
|
@ -98,12 +97,12 @@ namespace BasicGeometry
|
|||
this.x3 = quaternion.x3;
|
||||
}
|
||||
|
||||
public void SetConjugateOf(in QuaternionFP64 quaternion)
|
||||
public static void MakeConjugate(in QuaternionFP64 quaternion, out QuaternionFP64 conjugate)
|
||||
{
|
||||
this.s0 = quaternion.s0;
|
||||
this.x1 = -quaternion.x1;
|
||||
this.x2 = -quaternion.x2;
|
||||
this.x3 = -quaternion.x3;
|
||||
conjugate.s0 = quaternion.s0;
|
||||
conjugate.x1 = -quaternion.x1;
|
||||
conjugate.x2 = -quaternion.x2;
|
||||
conjugate.x3 = -quaternion.x3;
|
||||
}
|
||||
|
||||
public readonly void MakeRotationMatrix(out Matrix3x3FP64 matrix)
|
||||
|
|
@ -113,16 +112,15 @@ namespace BasicGeometry
|
|||
double x2x2 = this.x2 * this.x2;
|
||||
double x3x3 = this.x3 * this.x3;
|
||||
|
||||
double squareModule = (s0s0 + x1x1) + (x2x2 + x3x3);
|
||||
double squareModulus = (s0s0 + x1x1) + (x2x2 + x3x3);
|
||||
|
||||
if (squareModule <= UtilityFP64.SQUARE_EPSYLON)
|
||||
if (squareModulus <= UtilityFP64.SQUARE_EPSYLON || !double.IsFinite(squareModulus))
|
||||
{
|
||||
Matrix3x3FP64.LoadIdentity(out matrix);
|
||||
return;
|
||||
}
|
||||
|
||||
double corrector1 = 1.0 / squareModule;
|
||||
double corrector2 = 2.0 * corrector1;
|
||||
double corrector1 = 1.0 / squareModulus;
|
||||
|
||||
double s0x1 = this.s0 * this.x1;
|
||||
double s0x2 = this.s0 * this.x2;
|
||||
|
|
@ -131,6 +129,8 @@ namespace BasicGeometry
|
|||
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));
|
||||
|
|
@ -151,16 +151,15 @@ namespace BasicGeometry
|
|||
double x2x2 = this.x2 * this.x2;
|
||||
double x3x3 = this.x3 * this.x3;
|
||||
|
||||
double squareModule = (s0s0 + x1x1) + (x2x2 + x3x3);
|
||||
double squareModulus = (s0s0 + x1x1) + (x2x2 + x3x3);
|
||||
|
||||
if (squareModule <= UtilityFP64.SQUARE_EPSYLON)
|
||||
if (squareModulus <= UtilityFP64.SQUARE_EPSYLON || !double.IsFinite(squareModulus))
|
||||
{
|
||||
Matrix3x3FP64.LoadIdentity(out matrix);
|
||||
return;
|
||||
}
|
||||
|
||||
double corrector1 = 1.0 / squareModule;
|
||||
double corrector2 = 2.0 * corrector1;
|
||||
double corrector1 = 1.0 / squareModulus;
|
||||
|
||||
double s0x1 = this.s0 * this.x1;
|
||||
double s0x2 = this.s0 * this.x2;
|
||||
|
|
@ -169,6 +168,8 @@ namespace BasicGeometry
|
|||
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));
|
||||
|
|
@ -200,15 +201,33 @@ namespace BasicGeometry
|
|||
|
||||
public static void Multiply(in QuaternionFP64 left, in QuaternionFP64 right, out QuaternionFP64 product)
|
||||
{
|
||||
double s0 = (left.s0 * right.s0 - left.x1 * right.x1) - (left.x2 * right.x2 + left.x3 * right.x3);
|
||||
double x1 = (left.x1 * right.s0 + left.s0 * right.x1) - (left.x3 * right.x2 - left.x2 * right.x3);
|
||||
double x2 = (left.x2 * right.s0 + left.s0 * right.x2) - (left.x1 * right.x3 - left.x3 * right.x1);
|
||||
double x3 = (left.x3 * right.s0 + left.s0 * right.x3) - (left.x2 * right.x1 - left.x1 * right.x2);
|
||||
double s0 = left.s0 * right.s0 - left.x1 * right.x1 - (left.x2 * right.x2 + left.x3 * right.x3);
|
||||
double x1 = left.x1 * right.s0 + left.s0 * right.x1 - (left.x3 * right.x2 - left.x2 * right.x3);
|
||||
double x2 = left.x2 * right.s0 + left.s0 * right.x2 - (left.x1 * right.x3 - left.x3 * right.x1);
|
||||
double x3 = left.x3 * right.s0 + left.s0 * right.x3 - (left.x2 * right.x1 - left.x1 * right.x2);
|
||||
|
||||
product.s0 = s0;
|
||||
product.x1 = x1;
|
||||
product.x2 = x2;
|
||||
product.x3 = x3;
|
||||
}
|
||||
|
||||
public static bool AreClose(QuaternionFP32 quaternion1, QuaternionFP32 quaternion2)
|
||||
{
|
||||
double ds0 = quaternion1.s0 - quaternion2.s0;
|
||||
double dx1 = quaternion1.x1 - quaternion2.x1;
|
||||
double dx2 = quaternion1.x2 - quaternion2.x2;
|
||||
double dx3 = quaternion1.x3 - quaternion2.x3;
|
||||
|
||||
double squareModulus1 = quaternion1.GetSquareModulus();
|
||||
double squareModulus2 = quaternion2.GetSquareModulus();
|
||||
double squareDistance = (ds0 * ds0 + dx1 * dx1) + (dx2 * dx2 + dx3 * dx3);
|
||||
|
||||
if (squareModulus1 <= UtilityFP64.EPSYLON_EFFECTIVENESS_LIMIT || squareModulus2 <= UtilityFP64.EPSYLON_EFFECTIVENESS_LIMIT) {
|
||||
return squareDistance <= UtilityFP64.SQUARE_EPSYLON;
|
||||
}
|
||||
|
||||
return squareDistance <= UtilityFP64.SQUARE_EPSYLON * squareModulus1 && squareDistance <= UtilityFP64.SQUARE_EPSYLON * squareModulus2;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -26,7 +26,7 @@ namespace BasicGeometry
|
|||
return (1.0f - EPSYLON) <= value && value <= (1.0f + EPSYLON);
|
||||
}
|
||||
|
||||
public static bool IsSqareValueUnit(float square)
|
||||
public static bool IsSqareUnit(float square)
|
||||
{
|
||||
return (1.0f - 2.0f * EPSYLON) <= square && square <= (1.0f + 2.0f * EPSYLON);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -26,7 +26,7 @@ namespace BasicGeometry
|
|||
return (1.0 - EPSYLON) <= value && value <= (1.0 + EPSYLON);
|
||||
}
|
||||
|
||||
public static bool IsSqareValueUnit(double square)
|
||||
public static bool IsSqareUnit(double square)
|
||||
{
|
||||
return (1.0 - 2.0 * EPSYLON) <= square && square <= (1.0 + 2.0 * EPSYLON);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -48,32 +48,32 @@ namespace BasicGeometry
|
|||
this.x2 = (float)vector.x2;
|
||||
}
|
||||
|
||||
public readonly float GetSquareModule()
|
||||
public readonly float GetSquareModulus()
|
||||
{
|
||||
return this.x1 * this.x1 + this.x2 * this.x2;
|
||||
}
|
||||
|
||||
public readonly float GetModule()
|
||||
public readonly float GetModulus()
|
||||
{
|
||||
return MathF.Sqrt(this.GetSquareModule());
|
||||
return MathF.Sqrt(this.GetSquareModulus());
|
||||
}
|
||||
|
||||
public int Normalize()
|
||||
{
|
||||
float squareModule = this.GetSquareModule();
|
||||
float squareModulus = this.GetSquareModulus();
|
||||
|
||||
if (UtilityFP32.IsSqareValueUnit(squareModule))
|
||||
if (UtilityFP32.IsSqareUnit(squareModulus))
|
||||
{
|
||||
return 1;
|
||||
}
|
||||
|
||||
if (squareModule <= UtilityFP32.SQUARE_EPSYLON)
|
||||
if (squareModulus <= UtilityFP32.SQUARE_EPSYLON)
|
||||
{
|
||||
this.Reset();
|
||||
return 0;
|
||||
}
|
||||
|
||||
float multiplier = MathF.Sqrt(1.0f / squareModule);
|
||||
float multiplier = MathF.Sqrt(1.0f / squareModulus);
|
||||
|
||||
this.x1 *= multiplier;
|
||||
this.x2 *= multiplier;
|
||||
|
|
@ -89,12 +89,12 @@ namespace BasicGeometry
|
|||
|
||||
public readonly bool IsZero()
|
||||
{
|
||||
return this.GetSquareModule() <= UtilityFP32.SQUARE_EPSYLON;
|
||||
return this.GetSquareModulus() <= UtilityFP32.SQUARE_EPSYLON;
|
||||
}
|
||||
|
||||
public readonly bool IsUnit()
|
||||
{
|
||||
return UtilityFP32.IsSqareValueUnit(this.GetSquareModule());
|
||||
return UtilityFP32.IsSqareUnit(this.GetSquareModulus());
|
||||
}
|
||||
|
||||
public void Reset()
|
||||
|
|
@ -191,21 +191,21 @@ namespace BasicGeometry
|
|||
|
||||
public static float GetAngle(in Vector2FP32 vector1, in Vector2FP32 vector2, AngleUnit unit)
|
||||
{
|
||||
float squareModule1 = vector1.GetSquareModule();
|
||||
float squareModulus1 = vector1.GetSquareModulus();
|
||||
|
||||
if (squareModule1 <= UtilityFP32.SQUARE_EPSYLON)
|
||||
if (squareModulus1 <= UtilityFP32.SQUARE_EPSYLON)
|
||||
{
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
float squareModule2 = vector2.GetSquareModule();
|
||||
float squareModulus2 = vector2.GetSquareModulus();
|
||||
|
||||
if (squareModule2 <= UtilityFP32.SQUARE_EPSYLON)
|
||||
if (squareModulus2 <= UtilityFP32.SQUARE_EPSYLON)
|
||||
{
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
float cosine = Vector2FP32.GetScalarProduct(vector1, vector2) / MathF.Sqrt(squareModule1 * squareModule2);
|
||||
float cosine = Vector2FP32.GetScalarProduct(vector1, vector2) / MathF.Sqrt(squareModulus1 * squareModulus2);
|
||||
|
||||
if (1.0f - UtilityFP32.EPSYLON <= cosine)
|
||||
{
|
||||
|
|
@ -235,22 +235,22 @@ namespace BasicGeometry
|
|||
|
||||
public static bool AreEqual(in Vector2FP32 vector1, in Vector2FP32 vector2)
|
||||
{
|
||||
float squareModule1 = vector1.GetSquareModule();
|
||||
float squareModule2 = vector2.GetSquareModule();
|
||||
float squareModule3 = GetSquareDistance(vector1, vector2);
|
||||
float squareModulus1 = vector1.GetSquareModulus();
|
||||
float squareModulus2 = vector2.GetSquareModulus();
|
||||
float squareModulus3 = GetSquareDistance(vector1, vector2);
|
||||
|
||||
// 2.0f means dimension amount
|
||||
if (squareModule1 < UtilityFP32.EPSYLON_EFFECTIVENESS_LIMIT || squareModule2 < UtilityFP32.EPSYLON_EFFECTIVENESS_LIMIT)
|
||||
if (squareModulus1 < UtilityFP32.EPSYLON_EFFECTIVENESS_LIMIT || squareModulus2 < UtilityFP32.EPSYLON_EFFECTIVENESS_LIMIT)
|
||||
{
|
||||
return squareModule3 < (2.0f * UtilityFP32.SQUARE_EPSYLON);
|
||||
return squareModulus3 < (2.0f * UtilityFP32.SQUARE_EPSYLON);
|
||||
}
|
||||
|
||||
if (squareModule1 <= squareModule2)
|
||||
if (squareModulus1 <= squareModulus2)
|
||||
{
|
||||
return squareModule3 <= (2.0f * UtilityFP32.SQUARE_EPSYLON) * squareModule2;
|
||||
return squareModulus3 <= (2.0f * UtilityFP32.SQUARE_EPSYLON) * squareModulus2;
|
||||
}
|
||||
|
||||
return squareModule3 <= (2.0f * UtilityFP32.SQUARE_EPSYLON) * squareModule1;
|
||||
return squareModulus3 <= (2.0f * UtilityFP32.SQUARE_EPSYLON) * squareModulus1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -48,32 +48,32 @@ namespace BasicGeometry
|
|||
this.x2 = vector.x2;
|
||||
}
|
||||
|
||||
public readonly double GetSquareModule()
|
||||
public readonly double GetSquareModulus()
|
||||
{
|
||||
return this.x1 * this.x1 + this.x2 * this.x2;
|
||||
}
|
||||
|
||||
public readonly double GetModule()
|
||||
public readonly double GetModulus()
|
||||
{
|
||||
return Math.Sqrt(this.GetSquareModule());
|
||||
return Math.Sqrt(this.GetSquareModulus());
|
||||
}
|
||||
|
||||
public int Normalize()
|
||||
{
|
||||
double squareModule = this.GetSquareModule();
|
||||
double squareModulus = this.GetSquareModulus();
|
||||
|
||||
if (UtilityFP64.IsSqareValueUnit(squareModule))
|
||||
if (UtilityFP64.IsSqareUnit(squareModulus))
|
||||
{
|
||||
return 1;
|
||||
}
|
||||
|
||||
if (squareModule <= UtilityFP64.SQUARE_EPSYLON)
|
||||
if (squareModulus <= UtilityFP64.SQUARE_EPSYLON)
|
||||
{
|
||||
this.Reset();
|
||||
return 0;
|
||||
}
|
||||
|
||||
double multiplier = Math.Sqrt(1.0 / squareModule);
|
||||
double multiplier = Math.Sqrt(1.0 / squareModulus);
|
||||
|
||||
this.x1 *= multiplier;
|
||||
this.x2 *= multiplier;
|
||||
|
|
@ -89,12 +89,12 @@ namespace BasicGeometry
|
|||
|
||||
public readonly bool IsZero()
|
||||
{
|
||||
return this.GetSquareModule() <= UtilityFP64.SQUARE_EPSYLON;
|
||||
return this.GetSquareModulus() <= UtilityFP64.SQUARE_EPSYLON;
|
||||
}
|
||||
|
||||
public readonly bool IsUnit()
|
||||
{
|
||||
return UtilityFP64.IsSqareValueUnit(this.GetSquareModule());
|
||||
return UtilityFP64.IsSqareUnit(this.GetSquareModulus());
|
||||
}
|
||||
|
||||
public void Reset()
|
||||
|
|
@ -191,21 +191,21 @@ namespace BasicGeometry
|
|||
|
||||
public static double GetAngle(in Vector2FP64 vector1, in Vector2FP64 vector2, AngleUnit unit)
|
||||
{
|
||||
double squareModule1 = vector1.GetSquareModule();
|
||||
double squareModulus1 = vector1.GetSquareModulus();
|
||||
|
||||
if (squareModule1 <= UtilityFP64.SQUARE_EPSYLON)
|
||||
if (squareModulus1 <= UtilityFP64.SQUARE_EPSYLON)
|
||||
{
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
double squareModule2 = vector2.GetSquareModule();
|
||||
double squareModulus2 = vector2.GetSquareModulus();
|
||||
|
||||
if (squareModule2 <= UtilityFP64.SQUARE_EPSYLON)
|
||||
if (squareModulus2 <= UtilityFP64.SQUARE_EPSYLON)
|
||||
{
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
double cosine = Vector2FP64.GetScalarProduct(vector1, vector2) / Math.Sqrt(squareModule1 * squareModule2);
|
||||
double cosine = Vector2FP64.GetScalarProduct(vector1, vector2) / Math.Sqrt(squareModulus1 * squareModulus2);
|
||||
|
||||
if (1.0 - UtilityFP64.EPSYLON <= cosine)
|
||||
{
|
||||
|
|
@ -235,22 +235,22 @@ namespace BasicGeometry
|
|||
|
||||
public static bool AreEqual(in Vector2FP64 vector1, in Vector2FP64 vector2)
|
||||
{
|
||||
double squareModule1 = vector1.GetSquareModule();
|
||||
double squareModule2 = vector2.GetSquareModule();
|
||||
double squareModule3 = GetSquareDistance(vector1, vector2);
|
||||
double squareModulus1 = vector1.GetSquareModulus();
|
||||
double squareModulus2 = vector2.GetSquareModulus();
|
||||
double squareModulus3 = GetSquareDistance(vector1, vector2);
|
||||
|
||||
// 2.0 means dimension amount
|
||||
if (squareModule1 < UtilityFP64.EPSYLON_EFFECTIVENESS_LIMIT || squareModule2 < UtilityFP64.EPSYLON_EFFECTIVENESS_LIMIT)
|
||||
if (squareModulus1 < UtilityFP64.EPSYLON_EFFECTIVENESS_LIMIT || squareModulus2 < UtilityFP64.EPSYLON_EFFECTIVENESS_LIMIT)
|
||||
{
|
||||
return squareModule3 < (2.0 * UtilityFP64.SQUARE_EPSYLON);
|
||||
return squareModulus3 < (2.0 * UtilityFP64.SQUARE_EPSYLON);
|
||||
}
|
||||
|
||||
if (squareModule1 <= squareModule2)
|
||||
if (squareModulus1 <= squareModulus2)
|
||||
{
|
||||
return squareModule3 <= (2.0 * UtilityFP64.SQUARE_EPSYLON) * squareModule2;
|
||||
return squareModulus3 <= (2.0 * UtilityFP64.SQUARE_EPSYLON) * squareModulus2;
|
||||
}
|
||||
|
||||
return squareModule3 <= (2.0 * UtilityFP64.SQUARE_EPSYLON) * squareModule1;
|
||||
return squareModulus3 <= (2.0 * UtilityFP64.SQUARE_EPSYLON) * squareModulus1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -52,32 +52,32 @@ namespace BasicGeometry
|
|||
this.x3 = (float)vector.x3;
|
||||
}
|
||||
|
||||
public readonly float GetSquareModule()
|
||||
public readonly float GetSquareModulus()
|
||||
{
|
||||
return this.x1 * this.x1 + this.x2 * this.x2 + this.x3 * this.x3;
|
||||
}
|
||||
|
||||
public readonly float GetModule()
|
||||
public readonly float GetModulus()
|
||||
{
|
||||
return MathF.Sqrt(this.GetSquareModule());
|
||||
return MathF.Sqrt(this.GetSquareModulus());
|
||||
}
|
||||
|
||||
public int Normalize()
|
||||
{
|
||||
float squareModule = this.GetSquareModule();
|
||||
float squareModulus = this.GetSquareModulus();
|
||||
|
||||
if (UtilityFP32.IsSqareValueUnit(squareModule))
|
||||
if (UtilityFP32.IsSqareUnit(squareModulus))
|
||||
{
|
||||
return 1;
|
||||
}
|
||||
|
||||
if (squareModule <= UtilityFP32.SQUARE_EPSYLON)
|
||||
if (squareModulus <= UtilityFP32.SQUARE_EPSYLON)
|
||||
{
|
||||
this.Reset();
|
||||
return 0;
|
||||
}
|
||||
|
||||
float multiplier = MathF.Sqrt(1.0f / squareModule);
|
||||
float multiplier = MathF.Sqrt(1.0f / squareModulus);
|
||||
|
||||
this.x1 *= multiplier;
|
||||
this.x2 *= multiplier;
|
||||
|
|
@ -95,12 +95,12 @@ namespace BasicGeometry
|
|||
|
||||
public readonly bool IsZero()
|
||||
{
|
||||
return this.GetSquareModule() <= UtilityFP32.SQUARE_EPSYLON;
|
||||
return this.GetSquareModulus() <= UtilityFP32.SQUARE_EPSYLON;
|
||||
}
|
||||
|
||||
public readonly bool IsUnit()
|
||||
{
|
||||
return UtilityFP32.IsSqareValueUnit(this.GetSquareModule());
|
||||
return UtilityFP32.IsSqareUnit(this.GetSquareModulus());
|
||||
}
|
||||
|
||||
public void Reset()
|
||||
|
|
@ -233,21 +233,21 @@ namespace BasicGeometry
|
|||
|
||||
public static float GetAngle(in Vector3FP32 vector1, in Vector3FP32 vector2, AngleUnit unit)
|
||||
{
|
||||
float squareModule1 = vector1.GetSquareModule();
|
||||
float squareModulus1 = vector1.GetSquareModulus();
|
||||
|
||||
if (squareModule1 <= UtilityFP32.SQUARE_EPSYLON)
|
||||
if (squareModulus1 <= UtilityFP32.SQUARE_EPSYLON)
|
||||
{
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
float squareModule2 = vector2.GetSquareModule();
|
||||
float squareModulus2 = vector2.GetSquareModulus();
|
||||
|
||||
if (squareModule2 <= UtilityFP32.SQUARE_EPSYLON)
|
||||
if (squareModulus2 <= UtilityFP32.SQUARE_EPSYLON)
|
||||
{
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
float cosine = Vector3FP32.GetScalarProduct(vector1, vector2) / MathF.Sqrt(squareModule1 * squareModule2);
|
||||
float cosine = Vector3FP32.GetScalarProduct(vector1, vector2) / MathF.Sqrt(squareModulus1 * squareModulus2);
|
||||
|
||||
if (1.0f - UtilityFP32.EPSYLON <= cosine)
|
||||
{
|
||||
|
|
@ -278,22 +278,17 @@ namespace BasicGeometry
|
|||
|
||||
public static bool AreEqual(in Vector3FP32 vector1, in Vector3FP32 vector2)
|
||||
{
|
||||
float squareModule1 = vector1.GetSquareModule();
|
||||
float squareModule2 = vector2.GetSquareModule();
|
||||
float squareModule3 = GetSquareDistance(vector1, vector2);
|
||||
float squareModulus1 = vector1.GetSquareModulus();
|
||||
float squareModulus2 = vector2.GetSquareModulus();
|
||||
float squareDistance = GetSquareDistance(vector1, vector2);
|
||||
|
||||
// 3.0f means dimension amount
|
||||
if (squareModule1 < UtilityFP32.EPSYLON_EFFECTIVENESS_LIMIT || squareModule2 < UtilityFP32.EPSYLON_EFFECTIVENESS_LIMIT)
|
||||
if (squareModulus1 <= UtilityFP32.EPSYLON_EFFECTIVENESS_LIMIT || squareModulus2 <= UtilityFP32.EPSYLON_EFFECTIVENESS_LIMIT)
|
||||
{
|
||||
return squareModule3 < (3.0f * UtilityFP32.SQUARE_EPSYLON);
|
||||
return squareDistance <= UtilityFP32.SQUARE_EPSYLON;
|
||||
}
|
||||
|
||||
if (squareModule1 <= squareModule2)
|
||||
{
|
||||
return squareModule3 <= (3.0f * UtilityFP32.SQUARE_EPSYLON) * squareModule2;
|
||||
}
|
||||
|
||||
return squareModule3 <= (3.0f * UtilityFP32.SQUARE_EPSYLON) * squareModule1;
|
||||
return squareDistance <= UtilityFP32.SQUARE_EPSYLON * squareModulus1 && squareDistance <= UtilityFP32.SQUARE_EPSYLON * squareModulus2;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -52,33 +52,33 @@ namespace BasicGeometry
|
|||
this.x3 = vector.x3;
|
||||
}
|
||||
|
||||
public readonly double GetSquareModule()
|
||||
public readonly double GetSquareModulus()
|
||||
{
|
||||
return this.x1 * this.x1 + this.x2 * this.x2 + this.x3 * this.x3;
|
||||
}
|
||||
|
||||
public readonly double GetModule()
|
||||
public readonly double GetModulus()
|
||||
{
|
||||
return Math.Sqrt(this.GetSquareModule());
|
||||
return Math.Sqrt(this.GetSquareModulus());
|
||||
}
|
||||
|
||||
public int Normalize()
|
||||
{
|
||||
double squareModule = this.GetSquareModule();
|
||||
double squareModulus = this.GetSquareModulus();
|
||||
|
||||
|
||||
if (UtilityFP64.IsSqareValueUnit(squareModule))
|
||||
if (UtilityFP64.IsSqareUnit(squareModulus))
|
||||
{
|
||||
return 1;
|
||||
}
|
||||
|
||||
if (squareModule <= UtilityFP64.SQUARE_EPSYLON)
|
||||
if (squareModulus <= UtilityFP64.SQUARE_EPSYLON)
|
||||
{
|
||||
this.Reset();
|
||||
return 0;
|
||||
}
|
||||
|
||||
double multiplier = Math.Sqrt(1.0 / squareModule);
|
||||
double multiplier = Math.Sqrt(1.0 / squareModulus);
|
||||
|
||||
this.x1 *= multiplier;
|
||||
this.x2 *= multiplier;
|
||||
|
|
@ -96,12 +96,12 @@ namespace BasicGeometry
|
|||
|
||||
public readonly bool IsZero()
|
||||
{
|
||||
return this.GetSquareModule() <= UtilityFP64.SQUARE_EPSYLON;
|
||||
return this.GetSquareModulus() <= UtilityFP64.SQUARE_EPSYLON;
|
||||
}
|
||||
|
||||
public readonly bool IsUnit()
|
||||
{
|
||||
return UtilityFP64.IsSqareValueUnit(this.GetSquareModule());
|
||||
return UtilityFP64.IsSqareUnit(this.GetSquareModulus());
|
||||
}
|
||||
|
||||
public void Reset()
|
||||
|
|
@ -234,21 +234,21 @@ namespace BasicGeometry
|
|||
|
||||
public static double GetAngle(in Vector3FP64 vector1, in Vector3FP64 vector2, AngleUnit unit)
|
||||
{
|
||||
double squareModule1 = vector1.GetSquareModule();
|
||||
double squareModulus1 = vector1.GetSquareModulus();
|
||||
|
||||
if (squareModule1 <= UtilityFP64.SQUARE_EPSYLON)
|
||||
if (squareModulus1 <= UtilityFP64.SQUARE_EPSYLON)
|
||||
{
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
double squareModule2 = vector2.GetSquareModule();
|
||||
double squareModulus2 = vector2.GetSquareModulus();
|
||||
|
||||
if (squareModule2 <= UtilityFP64.SQUARE_EPSYLON)
|
||||
if (squareModulus2 <= UtilityFP64.SQUARE_EPSYLON)
|
||||
{
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
double cosine = Vector3FP64.GetScalarProduct(vector1, vector2) / Math.Sqrt(squareModule1 * squareModule2);
|
||||
double cosine = Vector3FP64.GetScalarProduct(vector1, vector2) / Math.Sqrt(squareModulus1 * squareModulus2);
|
||||
|
||||
if (1.0 - UtilityFP64.EPSYLON <= cosine)
|
||||
{
|
||||
|
|
@ -279,22 +279,16 @@ namespace BasicGeometry
|
|||
|
||||
public static bool AreEqual(in Vector3FP64 vector1, in Vector3FP64 vector2)
|
||||
{
|
||||
double squareModule1 = vector1.GetSquareModule();
|
||||
double squareModule2 = vector2.GetSquareModule();
|
||||
double squareModule3 = GetSquareDistance(vector1, vector2);
|
||||
double squareModulus1 = vector1.GetSquareModulus();
|
||||
double squareModulus2 = vector2.GetSquareModulus();
|
||||
double squareDistance = GetSquareDistance(vector1, vector2);
|
||||
|
||||
// 3.0 means dimension amount
|
||||
if (squareModule1 < UtilityFP64.EPSYLON_EFFECTIVENESS_LIMIT || squareModule2 < UtilityFP64.EPSYLON_EFFECTIVENESS_LIMIT)
|
||||
if (squareModulus1 <= UtilityFP64.EPSYLON_EFFECTIVENESS_LIMIT || squareModulus2 <= UtilityFP64.EPSYLON_EFFECTIVENESS_LIMIT)
|
||||
{
|
||||
return squareModule3 < (3.0 * UtilityFP64.SQUARE_EPSYLON);
|
||||
return squareDistance <= UtilityFP64.SQUARE_EPSYLON;
|
||||
}
|
||||
|
||||
if (squareModule1 <= squareModule2)
|
||||
{
|
||||
return squareModule3 <= (3.0 * UtilityFP64.SQUARE_EPSYLON) * squareModule2;
|
||||
}
|
||||
|
||||
return squareModule3 <= (3.0 * UtilityFP64.SQUARE_EPSYLON) * squareModule1;
|
||||
return squareDistance <= UtilityFP64.SQUARE_EPSYLON * squareModulus1 && squareDistance <= UtilityFP64.SQUARE_EPSYLON * squareModulus2;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -30,7 +30,17 @@ namespace BasicGeometry
|
|||
|
||||
public VersorFP32(float s0, float x1, float x2, float x3)
|
||||
{
|
||||
LoadValues(s0, x1, x2, x3, out this);
|
||||
this.s0 = s0;
|
||||
this.x1 = x1;
|
||||
this.x2 = x2;
|
||||
this.x3 = x3;
|
||||
|
||||
float squareModulus = this.s0 * this.s0 + this.x1 * this.x1 + (this.x2 * this.x2 + this.x3 * this.x3);
|
||||
|
||||
if (!UtilityFP32.IsSqareUnit(squareModulus))
|
||||
{
|
||||
this.Normalize(squareModulus);
|
||||
}
|
||||
}
|
||||
|
||||
public VersorFP32(in VersorFP32 versor)
|
||||
|
|
@ -47,6 +57,13 @@ namespace BasicGeometry
|
|||
this.x1 = (float)versor.GetX1();
|
||||
this.x2 = (float)versor.GetX2();
|
||||
this.x3 = (float)versor.GetX3();
|
||||
|
||||
float squareModulus = this.s0 * this.s0 + this.x1 * this.x1 + (this.x2 * this.x2 + this.x3 * this.x3);
|
||||
|
||||
if (!UtilityFP32.IsSqareUnit(squareModulus))
|
||||
{
|
||||
this.Normalize(squareModulus);
|
||||
}
|
||||
}
|
||||
|
||||
public readonly float GetScalar()
|
||||
|
|
@ -69,6 +86,20 @@ namespace BasicGeometry
|
|||
return this.x3;
|
||||
}
|
||||
|
||||
public readonly float GetAngle(AngleUnit unit)
|
||||
{
|
||||
if (this.s0 <= -(1.0f - UtilityFP32.EPSYLON) || 1.0f - UtilityFP32.EPSYLON <= this.s0) {
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
if (UtilityFP32.IsZero(this.s0))
|
||||
{
|
||||
return AngleFP32.GetHalfCircle(unit);
|
||||
}
|
||||
|
||||
return RadianFP32.ToUnits(2.0f * MathF.Acos(this.s0), unit);
|
||||
}
|
||||
|
||||
public readonly bool IsIdle()
|
||||
{
|
||||
return this.s0 <= -(1.0f - UtilityFP32.EPSYLON) || (1.0f - UtilityFP32.EPSYLON) <= this.s0;
|
||||
|
|
@ -84,7 +115,8 @@ namespace BasicGeometry
|
|||
|
||||
public void Shorten()
|
||||
{
|
||||
if (this.s0 < 0.0f) {
|
||||
if (this.s0 < 0.0f)
|
||||
{
|
||||
this.s0 = -this.s0;
|
||||
this.x1 = -this.x1;
|
||||
this.x2 = -this.x2;
|
||||
|
|
@ -99,76 +131,6 @@ namespace BasicGeometry
|
|||
this.x3 = -this.x3;
|
||||
}
|
||||
|
||||
public readonly float GetAngle(AngleUnit unit)
|
||||
{
|
||||
if (this.s0 <= -(1.0f - UtilityFP32.EPSYLON) || 1.0f - UtilityFP32.EPSYLON <= this.s0) {
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
if (UtilityFP32.IsZero(this.s0))
|
||||
{
|
||||
return AngleFP32.GetHalfCircle(unit);
|
||||
}
|
||||
|
||||
return RadianFP32.ToUnits(2.0f * MathF.Acos(this.s0), unit);
|
||||
}
|
||||
|
||||
public readonly void MakeRotationMatrix(out Matrix3x3FP32 matrix)
|
||||
{
|
||||
float s0s0 = this.s0 * this.s0;
|
||||
float x1x1 = this.x1 * this.x1;
|
||||
float x2x2 = this.x1 * this.x2;
|
||||
float x3x3 = this.x1 * this.x3;
|
||||
|
||||
float s0x1 = 2.0f * this.s0 * this.x1;
|
||||
float s0x2 = 2.0f * this.s0 * this.x2;
|
||||
float s0x3 = 2.0f * this.s0 * this.x3;
|
||||
|
||||
float x1x2 = 2.0f * this.x1 * this.x2;
|
||||
float x1x3 = 2.0f * this.x1 * this.x3;
|
||||
float x2x3 = 2.0f * this.x2 * this.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.r2c1 = x1x2 + s0x3;
|
||||
matrix.r3c2 = x2x3 + s0x1;
|
||||
matrix.r1c3 = x1x3 + s0x2;
|
||||
}
|
||||
|
||||
public readonly void MakeReverseMatrix(out Matrix3x3FP32 matrix)
|
||||
{
|
||||
float s0s0 = this.s0 * this.s0;
|
||||
float x1x1 = this.x1 * this.x1;
|
||||
float x2x2 = this.x1 * this.x2;
|
||||
float x3x3 = this.x1 * this.x3;
|
||||
|
||||
float s0x1 = 2.0f * this.s0 * this.x1;
|
||||
float s0x2 = 2.0f * this.s0 * this.x2;
|
||||
float s0x3 = 2.0f * this.s0 * this.x3;
|
||||
|
||||
float x1x2 = 2.0f * this.x1 * this.x2;
|
||||
float x1x3 = 2.0f * this.x1 * this.x3;
|
||||
float x2x3 = 2.0f * this.x2 * this.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.r2c1 = x1x2 - s0x3;
|
||||
matrix.r3c2 = x2x3 - s0x1;
|
||||
matrix.r1c3 = x1x3 - s0x2;
|
||||
}
|
||||
|
||||
public void SetValues(float s0, float x1, float x2, float x3)
|
||||
{
|
||||
this.s0 = s0;
|
||||
|
|
@ -176,15 +138,15 @@ namespace BasicGeometry
|
|||
this.x2 = x2;
|
||||
this.x3 = x3;
|
||||
|
||||
float squareModule = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3);
|
||||
float squareModulus = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3);
|
||||
|
||||
if (!UtilityFP32.IsSqareValueUnit(squareModule))
|
||||
if (!UtilityFP32.IsSqareUnit(squareModulus))
|
||||
{
|
||||
this.Normalize(squareModule);
|
||||
this.Normalize(squareModulus);
|
||||
}
|
||||
}
|
||||
|
||||
public void SetValues(in VersorFP32 versor)
|
||||
public void Set(in VersorFP32 versor)
|
||||
{
|
||||
this.s0 = versor.s0;
|
||||
this.x1 = versor.x1;
|
||||
|
|
@ -192,95 +154,20 @@ namespace BasicGeometry
|
|||
this.x3 = versor.x3;
|
||||
}
|
||||
|
||||
public void SetValues(in VersorFP64 versor)
|
||||
public void Set(in VersorFP64 versor)
|
||||
{
|
||||
this.SetValues(
|
||||
(float) versor.GetScalar(),
|
||||
(float) versor.GetX1(),
|
||||
(float) versor.GetX2(),
|
||||
(float) versor.GetX3());
|
||||
this.SetValues((float) versor.GetScalar(), (float) versor.GetX1(), (float) versor.GetX2(), (float) versor.GetX3());
|
||||
}
|
||||
|
||||
public void SetInverted(in VersorFP32 versor)
|
||||
private void Normalize(float squareModulus)
|
||||
{
|
||||
this.s0 = versor.s0;
|
||||
this.x1 = -versor.x1;
|
||||
this.x2 = -versor.x2;
|
||||
this.x3 = -versor.x3;
|
||||
}
|
||||
|
||||
public void SetInverted(in VersorFP64 versor)
|
||||
{
|
||||
this.SetValues(
|
||||
(float)versor.GetScalar(),
|
||||
(float)-versor.GetX1(),
|
||||
(float)-versor.GetX2(),
|
||||
(float)-versor.GetX3());
|
||||
}
|
||||
|
||||
public void SetShortened(in VersorFP32 versor)
|
||||
{
|
||||
if (versor.s0 < 0.0f)
|
||||
{
|
||||
this.s0 = -versor.s0;
|
||||
this.x1 = -versor.x1;
|
||||
this.x2 = -versor.x2;
|
||||
this.x3 = -versor.x3;
|
||||
}
|
||||
else
|
||||
{
|
||||
this.s0 = versor.s0;
|
||||
this.x1 = versor.x1;
|
||||
this.x2 = versor.x2;
|
||||
this.x3 = versor.x3;
|
||||
}
|
||||
}
|
||||
|
||||
public void SetShortened(in VersorFP64 versor)
|
||||
{
|
||||
this.SetValues(versor);
|
||||
this.Shorten();
|
||||
}
|
||||
|
||||
public readonly void Turn(in Vector3FP32 vector, out Vector3FP32 result)
|
||||
{
|
||||
float tx1 = 2.0f * (this.x2 * vector.x3 - this.x3 * vector.x2);
|
||||
float tx2 = 2.0f * (this.x3 * vector.x1 - this.x1 * vector.x3);
|
||||
float tx3 = 2.0f * (this.x1 * vector.x2 - this.x2 * vector.x1);
|
||||
|
||||
float x1 = (vector.x1 + tx1 * this.s0) + (this.x2 * tx3 - this.x3 * tx2);
|
||||
float x2 = (vector.x2 + tx2 * this.s0) + (this.x3 * tx1 - this.x1 * tx3);
|
||||
float x3 = (vector.x3 + tx3 * this.s0) + (this.x1 * tx2 - this.x2 * tx1);
|
||||
|
||||
result.x1 = x1;
|
||||
result.x2 = x2;
|
||||
result.x3 = x3;
|
||||
}
|
||||
|
||||
public readonly void TurnBack(in Vector3FP32 vector, out Vector3FP32 result)
|
||||
{
|
||||
float tx1 = 2.0f * (this.x2 * vector.x3 - this.x3 * vector.x2);
|
||||
float tx2 = 2.0f * (this.x3 * vector.x1 - this.x1 * vector.x3);
|
||||
float tx3 = 2.0f * (this.x1 * vector.x2 - this.x2 * vector.x1);
|
||||
|
||||
float x1 = (vector.x1 - tx1 * this.s0) + (this.x2 * tx3 - this.x3 * tx2);
|
||||
float x2 = (vector.x2 - tx2 * this.s0) + (this.x3 * tx1 - this.x1 * tx3);
|
||||
float x3 = (vector.x3 - tx3 * this.s0) + (this.x1 * tx2 - this.x2 * tx1);
|
||||
|
||||
result.x1 = x1;
|
||||
result.x2 = x2;
|
||||
result.x3 = x3;
|
||||
}
|
||||
|
||||
private void Normalize(float squareModule)
|
||||
{
|
||||
if (squareModule <= UtilityFP32.SQUARE_EPSYLON)
|
||||
if (squareModulus <= UtilityFP32.SQUARE_EPSYLON || !float.IsFinite(squareModulus))
|
||||
{
|
||||
this.Reset();
|
||||
return;
|
||||
}
|
||||
|
||||
float multiplier = MathF.Sqrt(1.0f / squareModule);
|
||||
float multiplier = MathF.Sqrt(1.0f / squareModulus);
|
||||
|
||||
this.s0 *= multiplier;
|
||||
this.x1 *= multiplier;
|
||||
|
|
@ -295,39 +182,149 @@ namespace BasicGeometry
|
|||
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 squareModule = s0 * s0 + x1 * x1 + (x2 * x2 + x3 * x3);
|
||||
float squareModulus = s0 * s0 + x1 * x1 + (x2 * x2 + x3 * x3);
|
||||
|
||||
result.s0 = s0;
|
||||
result.x1 = x1;
|
||||
result.x2 = x2;
|
||||
result.x3 = x3;
|
||||
|
||||
if (!UtilityFP32.IsSqareValueUnit(squareModule))
|
||||
if (!UtilityFP32.IsSqareUnit(squareModulus))
|
||||
{
|
||||
result.Normalize(squareModule);
|
||||
result.Normalize(squareModulus);
|
||||
}
|
||||
}
|
||||
|
||||
public static void LoadIdle(out VersorFP32 versor)
|
||||
public static void MakeInverted(in VersorFP32 versor, out VersorFP32 conjugate)
|
||||
{
|
||||
versor.s0 = 1.0f;
|
||||
versor.x1 = 0.0f;
|
||||
versor.x2 = 0.0f;
|
||||
versor.x3 = 0.0f;
|
||||
conjugate.s0 = versor.s0;
|
||||
conjugate.x1 = -versor.x1;
|
||||
conjugate.x2 = -versor.x2;
|
||||
conjugate.x3 = -versor.x3;
|
||||
}
|
||||
|
||||
public static void LoadValues(float s0, float x1, float x2, float x3, out VersorFP32 versor)
|
||||
public static void MakeShortened(in VersorFP32 versor, out VersorFP32 shortened)
|
||||
{
|
||||
versor.s0 = s0;
|
||||
versor.x1 = x1;
|
||||
versor.x2 = x2;
|
||||
versor.x3 = x3;
|
||||
if (versor.s0 < 0.0f) {
|
||||
shortened.s0 = -versor.s0;
|
||||
shortened.x1 = -versor.x1;
|
||||
shortened.x2 = -versor.x2;
|
||||
shortened.x3 = -versor.x3;
|
||||
}
|
||||
else {
|
||||
shortened.s0 = versor.s0;
|
||||
shortened.x1 = versor.x1;
|
||||
shortened.x2 = versor.x2;
|
||||
shortened.x3 = versor.x3;
|
||||
}
|
||||
}
|
||||
|
||||
float squareModule = s0 * s0 + x1 * x1 + (x2 * x2 + x3 * x3);
|
||||
|
||||
if (!UtilityFP32.IsSqareValueUnit(squareModule))
|
||||
public static void MakeRotationMatrix(in VersorFP32 versor, out Matrix3x3FP32 matrix)
|
||||
{
|
||||
versor.Normalize(squareModule);
|
||||
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 x1x2 = 2.0f * versor.x1 * versor.x2;
|
||||
float x1x3 = 2.0f * versor.x1 * versor.x3;
|
||||
float x2x3 = 2.0f * 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.r2c1 = x1x2 + s0x3;
|
||||
matrix.r3c2 = x2x3 + s0x1;
|
||||
matrix.r1c3 = x1x3 + s0x2;
|
||||
}
|
||||
|
||||
public static void MakeReverseMatrix(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 x1x2 = 2.0f * versor.x1 * versor.x2;
|
||||
float x1x3 = 2.0f * versor.x1 * versor.x3;
|
||||
float x2x3 = 2.0f * 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.r2c1 = x1x2 - s0x3;
|
||||
matrix.r3c2 = x2x3 - s0x1;
|
||||
matrix.r1c3 = x1x3 - s0x2;
|
||||
}
|
||||
|
||||
public static void Turn(in VersorFP32 versor, in Vector3FP32 vector, out Vector3FP32 result)
|
||||
{
|
||||
float tx1 = 2.0f * (versor.x2 * vector.x3 - versor.x3 * vector.x2);
|
||||
float tx2 = 2.0f * (versor.x3 * vector.x1 - versor.x1 * vector.x3);
|
||||
float tx3 = 2.0f * (versor.x1 * vector.x2 - versor.x2 * vector.x1);
|
||||
|
||||
float x1 = vector.x1 + tx1 * versor.s0 + (versor.x2 * tx3 - versor.x3 * tx2);
|
||||
float x2 = vector.x2 + tx2 * versor.s0 + (versor.x3 * tx1 - versor.x1 * tx3);
|
||||
float x3 = vector.x3 + tx3 * versor.s0 + (versor.x1 * tx2 - versor.x2 * tx1);
|
||||
|
||||
result.x1 = x1;
|
||||
result.x2 = x2;
|
||||
result.x3 = x3;
|
||||
}
|
||||
|
||||
public static void TurnBack(in VersorFP32 versor, in Vector3FP32 vector, out Vector3FP32 result)
|
||||
{
|
||||
float tx1 = 2.0f * (versor.x2 * vector.x3 - versor.x3 * vector.x2);
|
||||
float tx2 = 2.0f * (versor.x3 * vector.x1 - versor.x1 * vector.x3);
|
||||
float tx3 = 2.0f * (versor.x1 * vector.x2 - versor.x2 * vector.x1);
|
||||
|
||||
float x1 = vector.x1 - tx1 * versor.s0 + (versor.x2 * tx3 - versor.x3 * tx2);
|
||||
float x2 = vector.x2 - tx2 * versor.s0 + (versor.x3 * tx1 - versor.x1 * tx3);
|
||||
float x3 = vector.x3 - tx3 * versor.s0 + (versor.x1 * tx2 - versor.x2 * tx1);
|
||||
|
||||
result.x1 = x1;
|
||||
result.x2 = x2;
|
||||
result.x3 = x3;
|
||||
}
|
||||
|
||||
public static void LoadIdentity(out VersorFP32 result)
|
||||
{
|
||||
result.s0 = 1.0f;
|
||||
result.x1 = 0.0f;
|
||||
result.x2 = 0.0f;
|
||||
result.x3 = 0.0f;
|
||||
}
|
||||
|
||||
public static void LoadValues(float s0, float x1, float x2, float x3, out VersorFP32 result)
|
||||
{
|
||||
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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -14,7 +14,6 @@
|
|||
* limitations under the License.
|
||||
*/
|
||||
|
||||
|
||||
/*
|
||||
* Author: Andrey Pokidov
|
||||
* Date: 20 Oct 2024
|
||||
|
|
@ -31,7 +30,17 @@ namespace BasicGeometry
|
|||
|
||||
public VersorFP64(double s0, double x1, double x2, double x3)
|
||||
{
|
||||
LoadValues(s0, x1, x2, x3, out this);
|
||||
this.s0 = s0;
|
||||
this.x1 = x1;
|
||||
this.x2 = x2;
|
||||
this.x3 = x3;
|
||||
|
||||
double squareModulus = this.s0 * this.s0 + this.x1 * this.x1 + (this.x2 * this.x2 + this.x3 * this.x3);
|
||||
|
||||
if (!UtilityFP64.IsSqareUnit(squareModulus))
|
||||
{
|
||||
this.Normalize(squareModulus);
|
||||
}
|
||||
}
|
||||
|
||||
public VersorFP64(in VersorFP64 versor)
|
||||
|
|
@ -44,7 +53,17 @@ namespace BasicGeometry
|
|||
|
||||
public VersorFP64(in VersorFP32 versor)
|
||||
{
|
||||
LoadValues(versor.GetScalar(), versor.GetX1(), versor.GetX2(), versor.GetX3(), out this);
|
||||
this.s0 = versor.GetScalar();
|
||||
this.x1 = versor.GetX1();
|
||||
this.x2 = versor.GetX2();
|
||||
this.x3 = versor.GetX3();
|
||||
|
||||
double squareModulus = this.s0 * this.s0 + this.x1 * this.x1 + (this.x2 * this.x2 + this.x3 * this.x3);
|
||||
|
||||
if (!UtilityFP64.IsSqareUnit(squareModulus))
|
||||
{
|
||||
this.Normalize(squareModulus);
|
||||
}
|
||||
}
|
||||
|
||||
public readonly double GetScalar()
|
||||
|
|
@ -67,7 +86,21 @@ namespace BasicGeometry
|
|||
return this.x3;
|
||||
}
|
||||
|
||||
public readonly bool IsIdle()
|
||||
public readonly double GetAngle(AngleUnit unit)
|
||||
{
|
||||
if (this.s0 <= -(1.0 - UtilityFP64.EPSYLON) || 1.0 - UtilityFP64.EPSYLON <= this.s0) {
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
if (UtilityFP64.IsZero(this.s0))
|
||||
{
|
||||
return AngleFP64.GetHalfCircle(unit);
|
||||
}
|
||||
|
||||
return RadianFP64.ToUnits(2.0 * Math.Acos(this.s0), unit);
|
||||
}
|
||||
|
||||
public readonly bool IsIdentity()
|
||||
{
|
||||
return this.s0 <= -(1.0 - UtilityFP64.EPSYLON) || (1.0 - UtilityFP64.EPSYLON) <= this.s0;
|
||||
}
|
||||
|
|
@ -98,39 +131,112 @@ namespace BasicGeometry
|
|||
this.x3 = -this.x3;
|
||||
}
|
||||
|
||||
public readonly double GetAngle(AngleUnit unit)
|
||||
public void SetValues(double s0, double x1, double x2, double x3)
|
||||
{
|
||||
if (this.s0 <= -(1.0 - UtilityFP64.EPSYLON) || 1.0 - UtilityFP64.EPSYLON <= this.s0) {
|
||||
return 0.0;
|
||||
this.s0 = s0;
|
||||
this.x1 = x1;
|
||||
this.x2 = x2;
|
||||
this.x3 = x3;
|
||||
|
||||
double squareModulus = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3);
|
||||
|
||||
if (!UtilityFP64.IsSqareUnit(squareModulus))
|
||||
{
|
||||
this.Normalize(squareModulus);
|
||||
}
|
||||
}
|
||||
|
||||
if (UtilityFP64.IsZero(this.s0))
|
||||
public void Set(in VersorFP64 versor)
|
||||
{
|
||||
return AngleFP64.GetHalfCircle(unit);
|
||||
this.s0 = versor.s0;
|
||||
this.x1 = versor.x1;
|
||||
this.x2 = versor.x2;
|
||||
this.x3 = versor.x3;
|
||||
}
|
||||
|
||||
return RadianFP64.ToUnits(2.0 * Math.Acos(this.s0), unit);
|
||||
public void Set(in VersorFP32 versor)
|
||||
{
|
||||
this.SetValues(versor.GetScalar(), versor.GetX1(), versor.GetX2(), versor.GetX3());
|
||||
}
|
||||
|
||||
public readonly void MakeRotationMatrix(out Matrix3x3FP64 matrix)
|
||||
private void Normalize(double squareModulus)
|
||||
{
|
||||
double s0s0 = this.s0 * this.s0;
|
||||
double x1x1 = this.x1 * this.x1;
|
||||
double x2x2 = this.x1 * this.x2;
|
||||
double x3x3 = this.x1 * this.x3;
|
||||
if (squareModulus <= UtilityFP64.SQUARE_EPSYLON || !double.IsFinite(squareModulus))
|
||||
{
|
||||
this.Reset();
|
||||
return;
|
||||
}
|
||||
|
||||
double multiplier = Math.Sqrt(1.0 / squareModulus);
|
||||
|
||||
double s0x1 = 2.0 * this.s0 * this.x1;
|
||||
double s0x2 = 2.0 * this.s0 * this.x2;
|
||||
double s0x3 = 2.0 * this.s0 * this.x3;
|
||||
this.s0 *= multiplier;
|
||||
this.x1 *= multiplier;
|
||||
this.x2 *= multiplier;
|
||||
this.x3 *= multiplier;
|
||||
}
|
||||
|
||||
double x1x2 = 2.0 * this.x1 * this.x2;
|
||||
double x1x3 = 2.0 * this.x1 * this.x3;
|
||||
double x2x3 = 2.0 * this.x2 * this.x3;
|
||||
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);
|
||||
|
||||
matrix.r1c1 = (s0s0 + x1x1) - (x2x2 + x3x3);
|
||||
matrix.r2c2 = (s0s0 + x2x2) - (x1x1 + x3x3);
|
||||
matrix.r3c3 = (s0s0 + x3x3) - (x1x1 + x2x2);
|
||||
double squareModulus = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3);
|
||||
|
||||
result.s0 = s0;
|
||||
result.x1 = x1;
|
||||
result.x2 = x2;
|
||||
result.x3 = x3;
|
||||
|
||||
if (!UtilityFP64.IsSqareUnit(squareModulus))
|
||||
{
|
||||
result.Normalize(squareModulus);
|
||||
}
|
||||
}
|
||||
|
||||
public static void MakeInverted(in VersorFP64 versor, out VersorFP64 conjugate)
|
||||
{
|
||||
conjugate.s0 = versor.s0;
|
||||
conjugate.x1 = -versor.x1;
|
||||
conjugate.x2 = -versor.x2;
|
||||
conjugate.x3 = -versor.x3;
|
||||
}
|
||||
|
||||
public static void MakeShortened(in VersorFP64 versor, out VersorFP64 shortened)
|
||||
{
|
||||
if (versor.s0 < 0.0) {
|
||||
shortened.s0 = -versor.s0;
|
||||
shortened.x1 = -versor.x1;
|
||||
shortened.x2 = -versor.x2;
|
||||
shortened.x3 = -versor.x3;
|
||||
}
|
||||
else {
|
||||
shortened.s0 = versor.s0;
|
||||
shortened.x1 = versor.x1;
|
||||
shortened.x2 = versor.x2;
|
||||
shortened.x3 = versor.x3;
|
||||
}
|
||||
}
|
||||
|
||||
public static void MakeRotationMatrix(in VersorFP64 versor, out Matrix3x3FP64 matrix)
|
||||
{
|
||||
double s0s0 = versor.s0 * versor.s0;
|
||||
double x1x1 = versor.x1 * versor.x1;
|
||||
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 x1x2 = 2.0 * versor.x1 * versor.x2;
|
||||
double x1x3 = 2.0 * versor.x1 * versor.x3;
|
||||
double x2x3 = 2.0 * 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;
|
||||
|
|
@ -141,24 +247,24 @@ namespace BasicGeometry
|
|||
matrix.r1c3 = x1x3 + s0x2;
|
||||
}
|
||||
|
||||
public readonly void MakeReverseMatrix(out Matrix3x3FP64 matrix)
|
||||
public static void MakeReverseMatrix(in VersorFP64 versor, out Matrix3x3FP64 matrix)
|
||||
{
|
||||
double s0s0 = this.s0 * this.s0;
|
||||
double x1x1 = this.x1 * this.x1;
|
||||
double x2x2 = this.x1 * this.x2;
|
||||
double x3x3 = this.x1 * this.x3;
|
||||
double s0s0 = versor.s0 * versor.s0;
|
||||
double x1x1 = versor.x1 * versor.x1;
|
||||
double x2x2 = versor.x1 * versor.x2;
|
||||
double x3x3 = versor.x1 * versor.x3;
|
||||
|
||||
double s0x1 = 2.0 * this.s0 * this.x1;
|
||||
double s0x2 = 2.0 * this.s0 * this.x2;
|
||||
double s0x3 = 2.0 * this.s0 * this.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 x1x2 = 2.0 * this.x1 * this.x2;
|
||||
double x1x3 = 2.0 * this.x1 * this.x3;
|
||||
double x2x3 = 2.0 * this.x2 * this.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;
|
||||
|
||||
matrix.r1c1 = (s0s0 + x1x1) - (x2x2 + x3x3);
|
||||
matrix.r2c2 = (s0s0 + x2x2) - (x1x1 + x3x3);
|
||||
matrix.r3c3 = (s0s0 + x3x3) - (x1x1 + x2x2);
|
||||
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;
|
||||
|
|
@ -169,157 +275,56 @@ namespace BasicGeometry
|
|||
matrix.r1c3 = x1x3 - s0x2;
|
||||
}
|
||||
|
||||
public void SetValues(double s0, double x1, double x2, double x3)
|
||||
public static void Turn(in VersorFP64 versor, in Vector3FP64 vector, out Vector3FP64 result)
|
||||
{
|
||||
this.s0 = s0;
|
||||
this.x1 = x1;
|
||||
this.x2 = x2;
|
||||
this.x3 = x3;
|
||||
double tx1 = 2.0 * (versor.x2 * vector.x3 - versor.x3 * vector.x2);
|
||||
double tx2 = 2.0 * (versor.x3 * vector.x1 - versor.x1 * vector.x3);
|
||||
double tx3 = 2.0 * (versor.x1 * vector.x2 - versor.x2 * vector.x1);
|
||||
|
||||
double squareModule = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3);
|
||||
|
||||
if (!UtilityFP64.IsSqareValueUnit(squareModule))
|
||||
{
|
||||
this.Normalize(squareModule);
|
||||
}
|
||||
}
|
||||
|
||||
public void SetValues(in VersorFP64 versor)
|
||||
{
|
||||
this.s0 = versor.s0;
|
||||
this.x1 = versor.x1;
|
||||
this.x2 = versor.x2;
|
||||
this.x3 = versor.x3;
|
||||
}
|
||||
|
||||
public void SetValues(in VersorFP32 versor)
|
||||
{
|
||||
this.SetValues(versor.GetScalar(), versor.GetX1(), versor.GetX2(), versor.GetX3());
|
||||
}
|
||||
|
||||
public void SetShortened(in VersorFP64 versor)
|
||||
{
|
||||
if (versor.s0 < 0.0)
|
||||
{
|
||||
this.s0 = -versor.s0;
|
||||
this.x1 = -versor.x1;
|
||||
this.x2 = -versor.x2;
|
||||
this.x3 = -versor.x3;
|
||||
}
|
||||
else
|
||||
{
|
||||
this.s0 = versor.s0;
|
||||
this.x1 = versor.x1;
|
||||
this.x2 = versor.x2;
|
||||
this.x3 = versor.x3;
|
||||
}
|
||||
}
|
||||
|
||||
public void SetShortened(in VersorFP32 versor)
|
||||
{
|
||||
this.SetValues(versor);
|
||||
this.Shorten();
|
||||
}
|
||||
|
||||
public void SetInverted(in VersorFP64 versor)
|
||||
{
|
||||
this.s0 = versor.s0;
|
||||
this.x1 = -versor.x1;
|
||||
this.x2 = -versor.x2;
|
||||
this.x3 = -versor.x3;
|
||||
}
|
||||
|
||||
public void SetInverted(in VersorFP32 versor)
|
||||
{
|
||||
this.SetValues(versor.GetScalar(), -versor.GetX1(), -versor.GetX2(), -versor.GetX3());
|
||||
}
|
||||
|
||||
public readonly void Turn(in Vector3FP64 vector, out Vector3FP64 result)
|
||||
{
|
||||
double tx1 = 2.0 * (this.x2 * vector.x3 - this.x3 * vector.x2);
|
||||
double tx2 = 2.0 * (this.x3 * vector.x1 - this.x1 * vector.x3);
|
||||
double tx3 = 2.0 * (this.x1 * vector.x2 - this.x2 * vector.x1);
|
||||
|
||||
double x1 = (vector.x1 + tx1 * this.s0) + (this.x2 * tx3 - this.x3 * tx2);
|
||||
double x2 = (vector.x2 + tx2 * this.s0) + (this.x3 * tx1 - this.x1 * tx3);
|
||||
double x3 = (vector.x3 + tx3 * this.s0) + (this.x1 * tx2 - this.x2 * tx1);
|
||||
double x1 = vector.x1 + tx1 * versor.s0 + (versor.x2 * tx3 - versor.x3 * tx2);
|
||||
double x2 = vector.x2 + tx2 * versor.s0 + (versor.x3 * tx1 - versor.x1 * tx3);
|
||||
double x3 = vector.x3 + tx3 * versor.s0 + (versor.x1 * tx2 - versor.x2 * tx1);
|
||||
|
||||
result.x1 = x1;
|
||||
result.x2 = x2;
|
||||
result.x3 = x3;
|
||||
}
|
||||
|
||||
public readonly void TurnBack(in Vector3FP64 vector, out Vector3FP64 result)
|
||||
public static void TurnBack(in VersorFP64 versor, in Vector3FP64 vector, out Vector3FP64 result)
|
||||
{
|
||||
double tx1 = 2.0 * (this.x2 * vector.x3 - this.x3 * vector.x2);
|
||||
double tx2 = 2.0 * (this.x3 * vector.x1 - this.x1 * vector.x3);
|
||||
double tx3 = 2.0 * (this.x1 * vector.x2 - this.x2 * vector.x1);
|
||||
double tx1 = 2.0 * (versor.x2 * vector.x3 - versor.x3 * vector.x2);
|
||||
double tx2 = 2.0 * (versor.x3 * vector.x1 - versor.x1 * vector.x3);
|
||||
double tx3 = 2.0 * (versor.x1 * vector.x2 - versor.x2 * vector.x1);
|
||||
|
||||
double x1 = (vector.x1 - tx1 * this.s0) + (this.x2 * tx3 - this.x3 * tx2);
|
||||
double x2 = (vector.x2 - tx2 * this.s0) + (this.x3 * tx1 - this.x1 * tx3);
|
||||
double x3 = (vector.x3 - tx3 * this.s0) + (this.x1 * tx2 - this.x2 * tx1);
|
||||
double x1 = vector.x1 - tx1 * versor.s0 + (versor.x2 * tx3 - versor.x3 * tx2);
|
||||
double x2 = vector.x2 - tx2 * versor.s0 + (versor.x3 * tx1 - versor.x1 * tx3);
|
||||
double x3 = vector.x3 - tx3 * versor.s0 + (versor.x1 * tx2 - versor.x2 * tx1);
|
||||
|
||||
result.x1 = x1;
|
||||
result.x2 = x2;
|
||||
result.x3 = x3;
|
||||
}
|
||||
|
||||
private void Normalize(double squareModule)
|
||||
public static void LoadIdentity(out VersorFP64 result)
|
||||
{
|
||||
if (squareModule <= UtilityFP64.SQUARE_EPSYLON || (this.x1 * this.x1 + this.x2 * this.x2 + this.x3 * this.x3) <= UtilityFP64.SQUARE_EPSYLON * squareModule)
|
||||
{
|
||||
this.Reset();
|
||||
return;
|
||||
result.s0 = 1.0;
|
||||
result.x1 = 0.0;
|
||||
result.x2 = 0.0;
|
||||
result.x3 = 0.0;
|
||||
}
|
||||
|
||||
double multiplier = Math.Sqrt(1.0 / squareModule);
|
||||
|
||||
this.s0 *= multiplier;
|
||||
this.x1 *= multiplier;
|
||||
this.x2 *= multiplier;
|
||||
this.x3 *= multiplier;
|
||||
}
|
||||
|
||||
public static void Combine(in VersorFP64 second, in VersorFP64 first, out VersorFP64 result)
|
||||
public static void LoadValues(double s0, double x1, double x2, double x3, 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);
|
||||
|
||||
double squareModule = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3);
|
||||
double squareModulus = s0 * s0 + x1 * x1 + (x2 * x2 + x3 * x3);
|
||||
|
||||
result.s0 = s0;
|
||||
result.x1 = x1;
|
||||
result.x2 = x2;
|
||||
result.x3 = x3;
|
||||
|
||||
if (!UtilityFP64.IsSqareValueUnit(squareModule))
|
||||
if (!UtilityFP64.IsSqareUnit(squareModulus))
|
||||
{
|
||||
result.Normalize(squareModule);
|
||||
}
|
||||
}
|
||||
|
||||
public static void LoadIdle(out VersorFP64 versor)
|
||||
{
|
||||
versor.s0 = 1.0;
|
||||
versor.x1 = 0.0;
|
||||
versor.x2 = 0.0;
|
||||
versor.x3 = 0.0;
|
||||
}
|
||||
|
||||
public static void LoadValues(double s0, double x1, double x2, double x3, out VersorFP64 versor)
|
||||
{
|
||||
versor.s0 = s0;
|
||||
versor.x1 = x1;
|
||||
versor.x2 = x2;
|
||||
versor.x3 = x3;
|
||||
|
||||
double squareModule = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3);
|
||||
|
||||
if (!UtilityFP64.IsSqareValueUnit(squareModule))
|
||||
{
|
||||
versor.Normalize(squareModule);
|
||||
result.Normalize(squareModulus);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -2,6 +2,7 @@
|
|||
using System;
|
||||
using System.ComponentModel;
|
||||
using System.Diagnostics;
|
||||
using System.Numerics;
|
||||
using BasicGeometry;
|
||||
|
||||
public static class Program
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue