Kimelas/bgc-net
2
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Задача 0000001: отказ от использования корректоров в версорах / Task 0000001: declining of usage of correctors in versors

Browse source
This commit is contained in:
Andrey Pokidov 2024-11-19 19:15:34 +07:00
parent 72d06a23b2
commit f9bf5ef92e
3 changed files with 184 additions and 277 deletions
Download patch file Download diff file Expand all files Collapse all files

220
BGC/F32Versor.cs
View file

@ -23,22 +23,10 @@ namespace BGC
{
public struct F32Versor
{
public static readonly F32Versor IDLE = new F32Versor(1.0f, 0.0f, 0.0f, 0.0f, 1.0f);
private float s0;
private float x1;
private float x2;
private float x3;
private float corrector;
private F32Versor(float s0, float x1, float x2, float x3, float corrector)
{
this.s0 = s0;
this.x1 = x1;
this.x2 = x2;
this.x3 = x3;
this.corrector = corrector;
}
public F32Versor()
{
@ -46,28 +34,11 @@ namespace BGC
this.x1 = 0.0f;
this.x2 = 0.0f;
this.x3 = 0.0f;
this.corrector = 1.0f;
}
public F32Versor(float s0, float x1, float x2, float x3)
{
this.s0 = s0;
this.x1 = x1;
this.x2 = x2;
this.x3 = x3;
float squareModule = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3);
this.corrector = 2.0f - squareModule;
if (squareModule < 1.0f - F32Utility.TWO_EPSYLON || 1.0f + F32Utility.TWO_EPSYLON < squareModule)
{
this.Normalize(squareModule);
}
else if (s0 < -1.0f + F32Utility.EPSYLON || 1.0f - F32Utility.EPSYLON < s0)
{
this.Reset();
}
LoadValues(s0, x1, x2, x3, out this);
}
public F32Versor(in F32Versor versor)
@ -76,7 +47,6 @@ namespace BGC
this.x1 = versor.x1;
this.x2 = versor.x2;
this.x3 = versor.x3;
this.corrector = versor.corrector;
}
public F32Versor(in F64Versor versor)
@ -85,19 +55,6 @@ namespace BGC
this.x1 = (float)versor.GetX1();
this.x2 = (float)versor.GetX2();
this.x3 = (float)versor.GetX3();
float squareModule = (this.s0 * this.s0 + this.x1 * this.x1) + (this.x2 * this.x2 + this.x3 * this.x3);
this.corrector = 2.0f - squareModule;
if (squareModule < 1.0f - F32Utility.TWO_EPSYLON || 1.0f + F32Utility.TWO_EPSYLON < squareModule)
{
this.Normalize(squareModule);
}
else if (s0 < -1.0f + F32Utility.EPSYLON || 1.0f - F32Utility.EPSYLON < s0)
{
this.Reset();
}
}
public readonly float GetScalar()
@ -120,11 +77,6 @@ namespace BGC
return this.x3;
}
public readonly float GetCorrector()
{
return this.corrector;
}
public readonly bool IsIdle()
{
return this.s0 <= -(1.0f - F32Utility.EPSYLON) || (1.0f - F32Utility.EPSYLON) <= this.s0;
@ -136,7 +88,6 @@ namespace BGC
this.x1 = 0.0f;
this.x2 = 0.0f;
this.x3 = 0.0f;
this.corrector = 1.0f;
}
public void Invert()
@ -167,28 +118,25 @@ namespace BGC
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 s0x1 = this.s0 * this.x1;
float s0x2 = this.s0 * this.x2;
float s0x3 = 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;
float x1x2 = this.x1 * this.x2;
float x1x3 = this.x1 * this.x3;
float x2x3 = this.x2 * this.x3;
matrix.r1c1 = (s0s0 + x1x1) - (x2x2 + x3x3);
matrix.r2c2 = (s0s0 + x2x2) - (x1x1 + x3x3);
matrix.r3c3 = (s0s0 + x3x3) - (x1x1 + x2x2);
float corrector2 = 2.0f * this.corrector;
matrix.r1c2 = x1x2 - s0x3;
matrix.r2c3 = x2x3 - s0x1;
matrix.r3c1 = x1x3 - s0x2;
matrix.r1c1 = this.corrector * ((s0s0 + x1x1) - (x2x2 + x3x3));
matrix.r2c2 = this.corrector * ((s0s0 + x2x2) - (x1x1 + x3x3));
matrix.r3c3 = this.corrector * ((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);
matrix.r2c1 = x1x2 + s0x3;
matrix.r3c2 = x2x3 + s0x1;
matrix.r1c3 = x1x3 + s0x2;
}
public readonly void MakeReverseMatrix(out F32Matrix3x3 matrix)
@ -198,28 +146,25 @@ namespace BGC
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 s0x1 = this.s0 * this.x1;
float s0x2 = this.s0 * this.x2;
float s0x3 = 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;
float x1x2 = this.x1 * this.x2;
float x1x3 = this.x1 * this.x3;
float x2x3 = this.x2 * this.x3;
matrix.r1c1 = (s0s0 + x1x1) - (x2x2 + x3x3);
matrix.r2c2 = (s0s0 + x2x2) - (x1x1 + x3x3);
matrix.r3c3 = (s0s0 + x3x3) - (x1x1 + x2x2);
float corrector2 = 2.0f * this.corrector;
matrix.r1c2 = x1x2 + s0x3;
matrix.r2c3 = x2x3 + s0x1;
matrix.r3c1 = x1x3 + s0x2;
matrix.r1c1 = this.corrector * ((s0s0 + x1x1) - (x2x2 + x3x3));
matrix.r2c2 = this.corrector * ((s0s0 + x2x2) - (x1x1 + x3x3));
matrix.r3c3 = this.corrector * ((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);
matrix.r2c1 = x1x2 - s0x3;
matrix.r3c2 = x2x3 - s0x1;
matrix.r1c3 = x1x3 - s0x2;
}
public void SetValues(float s0, float x1, float x2, float x3)
@ -228,21 +173,21 @@ namespace BGC
this.x1 = x1;
this.x2 = x2;
this.x3 = x3;
float squareModule = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3);
if (squareModule < 1.0f - F32Utility.TWO_EPSYLON || 1.0f + F32Utility.TWO_EPSYLON < squareModule)
{
this.Normalize(squareModule);
return;
}
float squareModule = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3);
if (s0 < -1.0f + F32Utility.EPSYLON || 1.0f - F32Utility.EPSYLON < s0) {
if (1.0f - F32Utility.TWO_EPSYLON <= squareModule && squareModule <= 1.0f + F32Utility.TWO_EPSYLON)
{
if (-1.0f + F32Utility.EPSYLON < s0)
{
return;
}
this.Reset();
return;
}
this.corrector = 2.0f - squareModule;
this.Normalize(squareModule);
}
public void SetValues(in F32Versor versor)
@ -251,17 +196,14 @@ namespace BGC
this.x1 = versor.x1;
this.x2 = versor.x2;
this.x3 = versor.x3;
this.corrector = versor.corrector;
}
public void SetValues(in F64Versor versor)
{
this.SetValues(
(float)versor.GetScalar(),
(float)versor.GetX1(),
(float)versor.GetX2(),
(float)versor.GetX3()
);
this.s0 = (float) versor.GetScalar();
this.x1 = (float) versor.GetX1();
this.x2 = (float) versor.GetX2();
this.x3 = (float) versor.GetX3();
}
public void SetInverted(in F32Versor versor)
@ -270,25 +212,21 @@ namespace BGC
this.x1 = -versor.x1;
this.x2 = -versor.x2;
this.x3 = -versor.x3;
this.corrector = versor.corrector;
}
public void SetInverted(in F64Versor versor)
{
this.SetValues(
(float)versor.GetScalar(),
-(float)versor.GetX1(),
-(float)versor.GetX2(),
-(float)versor.GetX3()
);
this.s0 = (float) versor.GetScalar();
this.x1 = (float) -versor.GetX1();
this.x2 = (float) -versor.GetX2();
this.x3 = (float) -versor.GetX3();
}
public readonly void Turn(in F32Vector3 vector, out F32Vector3 result)
{
float multiplier = 2.0f * this.corrector;
float tx1 = multiplier * (this.x2 * vector.x3 - this.x3 * vector.x2);
float tx2 = multiplier * (this.x3 * vector.x1 - this.x1 * vector.x3);
float tx3 = multiplier * (this.x1 * vector.x2 - this.x2 * vector.x1);
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);
@ -301,10 +239,9 @@ namespace BGC
public readonly void TurnBack(in F32Vector3 vector, out F32Vector3 result)
{
float multiplier = 2.0f * this.corrector;
float tx1 = multiplier * (this.x2 * vector.x3 - this.x3 * vector.x2);
float tx2 = multiplier * (this.x3 * vector.x1 - this.x1 * vector.x3);
float tx3 = multiplier * (this.x1 * vector.x2 - this.x2 * vector.x1);
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);
@ -329,38 +266,47 @@ namespace BGC
this.x1 /= module;
this.x2 /= module;
this.x3 /= module;
this.corrector = (this.s0 * this.s0 + this.x1 * this.x1) + (this.x2 * this.x2 + this.x3 * this.x3);
}
public static void Combine(in F32Versor second, in F32Versor first, out F32Versor 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 squareModule = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3);
result.corrector = 2.0f - squareModule;
result.s0 = s0;
result.x1 = x1;
result.x2 = x2;
result.x3 = x3;
if (squareModule < 1.0f - F32Utility.TWO_EPSYLON || 1.0f + F32Utility.TWO_EPSYLON < squareModule)
{
result.Normalize(squareModule);
}
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 LoadIdle(out F32Versor versor)
{
versor.corrector = 1.0f;
versor.s0 = 1.0f;
versor.x1 = 0.0f;
versor.x2 = 0.0f;
versor.x3 = 0.0f;
}
public static void LoadValues(float s0, float x1, float x2, float x3, out F32Versor versor)
{
versor.s0 = s0;
versor.x1 = x1;
versor.x2 = x2;
versor.x3 = x3;
float squareModule = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3);
if (1.0f - F32Utility.TWO_EPSYLON <= squareModule && squareModule <= 1.0f + F32Utility.TWO_EPSYLON)
{
if (-1.0f + F32Utility.EPSYLON < s0)
{
return;
}
versor.Reset();
return;
}
versor.Normalize(squareModule);
}
}
}