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

237
BGC/F64Versor.cs
View file

@ -24,22 +24,10 @@ namespace BGC
{ {
public struct F64Versor public struct F64Versor
{ {
public static readonly F64Versor IDLE = new F64Versor(1.0, 0.0, 0.0, 0.0, 1.0);
private double s0; private double s0;
private double x1; private double x1;
private double x2; private double x2;
private double x3; private double x3;
private double corrector;
private F64Versor(double s0, double x1, double x2, double x3, double corrector)
{
this.s0 = s0;
this.x1 = x1;
this.x2 = x2;
this.x3 = x3;
this.corrector = corrector;
}
public F64Versor() public F64Versor()
{ {
@ -47,24 +35,11 @@ namespace BGC
this.x1 = 0.0; this.x1 = 0.0;
this.x2 = 0.0; this.x2 = 0.0;
this.x3 = 0.0; this.x3 = 0.0;
this.corrector = 1.0;
} }
public F64Versor(double s0, double x1, double x2, double x3) public F64Versor(double s0, double x1, double x2, double x3)
{ {
this.s0 = s0; LoadValues(s0, x1, x2, x3, out this);
this.x1 = x1;
this.x2 = x2;
this.x3 = x3;
double squareModule = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3);
this.corrector = 2.0 - squareModule;
if (squareModule < 1.0 - F64Utility.TWO_EPSYLON || 1.0 + F64Utility.TWO_EPSYLON < squareModule)
{
this.Normalize(squareModule);
}
} }
public F64Versor(in F64Versor versor) public F64Versor(in F64Versor versor)
@ -73,24 +48,16 @@ namespace BGC
this.x1 = versor.x1; this.x1 = versor.x1;
this.x2 = versor.x2; this.x2 = versor.x2;
this.x3 = versor.x3; this.x3 = versor.x3;
this.corrector = versor.corrector;
} }
public F64Versor(in F32Versor versor) public F64Versor(in F32Versor versor)
{ {
this.s0 = versor.GetScalar(); LoadValues(
this.x1 = versor.GetX1(); versor.GetScalar(),
this.x2 = versor.GetX2(); versor.GetX1(),
this.x3 = versor.GetX3(); versor.GetX2(),
versor.GetX3(),
double squareModule = (this.s0 * this.s0 + this.x1 * this.x1) + (this.x2 * this.x2 + this.x3 * this.x3); out this);
this.corrector = 2.0 - squareModule;
if (squareModule < 1.0 - F64Utility.TWO_EPSYLON || 1.0 + F64Utility.TWO_EPSYLON < squareModule)
{
this.Normalize(squareModule);
}
} }
public readonly double GetScalar() public readonly double GetScalar()
@ -113,11 +80,6 @@ namespace BGC
return this.x3; return this.x3;
} }
public readonly double GetCorrector()
{
return this.corrector;
}
public readonly bool IsIdle() public readonly bool IsIdle()
{ {
return this.s0 <= -(1.0 - F64Utility.EPSYLON) || (1.0 - F64Utility.EPSYLON) <= this.s0; return this.s0 <= -(1.0 - F64Utility.EPSYLON) || (1.0 - F64Utility.EPSYLON) <= this.s0;
@ -129,7 +91,6 @@ namespace BGC
this.x1 = 0.0; this.x1 = 0.0;
this.x2 = 0.0; this.x2 = 0.0;
this.x3 = 0.0; this.x3 = 0.0;
this.corrector = 1.0;
} }
public void Invert() public void Invert()
@ -161,27 +122,25 @@ namespace BGC
double x3x3 = this.x1 * this.x3; double x3x3 = this.x1 * this.x3;
double s0x1 = this.s0 * this.x1; double s0x1 = 2.0 * this.s0 * this.x1;
double s0x2 = this.s0 * this.x2; double s0x2 = 2.0 * this.s0 * this.x2;
double s0x3 = this.s0 * this.x3; double s0x3 = 2.0 * this.s0 * this.x3;
double x1x2 = this.x1 * this.x2; double x1x2 = 2.0 * this.x1 * this.x2;
double x1x3 = this.x1 * this.x3; double x1x3 = 2.0 * this.x1 * this.x3;
double x2x3 = this.x2 * this.x3; double x2x3 = 2.0 * this.x2 * this.x3;
double corrector2 = 2.0 * this.corrector; matrix.r1c1 = (s0s0 + x1x1) - (x2x2 + x3x3);
matrix.r2c2 = (s0s0 + x2x2) - (x1x1 + x3x3);
matrix.r3c3 = (s0s0 + x3x3) - (x1x1 + x2x2);
matrix.r1c1 = this.corrector * ((s0s0 + x1x1) - (x2x2 + x3x3)); matrix.r1c2 = x1x2 - s0x3;
matrix.r2c2 = this.corrector * ((s0s0 + x2x2) - (x1x1 + x3x3)); matrix.r2c3 = x2x3 - s0x1;
matrix.r3c3 = this.corrector * ((s0s0 + x3x3) - (x1x1 + x2x2)); matrix.r3c1 = x1x3 - s0x2;
matrix.r1c2 = corrector2 * (x1x2 - s0x3); matrix.r2c1 = x1x2 + s0x3;
matrix.r2c3 = corrector2 * (x2x3 - s0x1); matrix.r3c2 = x2x3 + s0x1;
matrix.r3c1 = corrector2 * (x1x3 - s0x2); matrix.r1c3 = x1x3 + s0x2;
matrix.r2c1 = corrector2 * (x1x2 + s0x3);
matrix.r3c2 = corrector2 * (x2x3 + s0x1);
matrix.r1c3 = corrector2 * (x1x3 + s0x2);
} }
public readonly void MakeReverseMatrix(out F64Matrix3x3 matrix) public readonly void MakeReverseMatrix(out F64Matrix3x3 matrix)
@ -191,28 +150,25 @@ namespace BGC
double x2x2 = this.x1 * this.x2; double x2x2 = this.x1 * this.x2;
double x3x3 = this.x1 * this.x3; double x3x3 = this.x1 * this.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 = this.s0 * this.x1; double x1x2 = 2.0 * this.x1 * this.x2;
double s0x2 = this.s0 * this.x2; double x1x3 = 2.0 * this.x1 * this.x3;
double s0x3 = this.s0 * this.x3; double x2x3 = 2.0 * this.x2 * this.x3;
double x1x2 = this.x1 * this.x2; matrix.r1c1 = (s0s0 + x1x1) - (x2x2 + x3x3);
double x1x3 = this.x1 * this.x3; matrix.r2c2 = (s0s0 + x2x2) - (x1x1 + x3x3);
double x2x3 = this.x2 * this.x3; matrix.r3c3 = (s0s0 + x3x3) - (x1x1 + x2x2);
double corrector2 = 2.0 * this.corrector; matrix.r1c2 = x1x2 + s0x3;
matrix.r2c3 = x2x3 + s0x1;
matrix.r3c1 = x1x3 + s0x2;
matrix.r1c1 = this.corrector * ((s0s0 + x1x1) - (x2x2 + x3x3)); matrix.r2c1 = x1x2 - s0x3;
matrix.r2c2 = this.corrector * ((s0s0 + x2x2) - (x1x1 + x3x3)); matrix.r3c2 = x2x3 - s0x1;
matrix.r3c3 = this.corrector * ((s0s0 + x3x3) - (x1x1 + x2x2)); matrix.r1c3 = x1x3 - s0x2;
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);
} }
public void SetValues(double s0, double x1, double x2, double x3) public void SetValues(double s0, double x1, double x2, double x3)
@ -223,13 +179,19 @@ namespace BGC
this.x3 = x3; this.x3 = x3;
double squareModule = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3); double squareModule = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3);
this.corrector = 2.0 - squareModule; if (1.0 - F64Utility.TWO_EPSYLON <= squareModule && squareModule <= 1.0 + F64Utility.TWO_EPSYLON)
if (squareModule < 1.0 - F64Utility.TWO_EPSYLON || 1.0 + F64Utility.TWO_EPSYLON < squareModule)
{ {
this.Normalize(squareModule); if (-1.0 + F64Utility.EPSYLON < s0)
{
return;
}
this.Reset();
return;
} }
this.Normalize(squareModule);
} }
public void SetValues(in F64Versor versor) public void SetValues(in F64Versor versor)
@ -238,24 +200,16 @@ namespace BGC
this.x1 = versor.x1; this.x1 = versor.x1;
this.x2 = versor.x2; this.x2 = versor.x2;
this.x3 = versor.x3; this.x3 = versor.x3;
this.corrector = versor.corrector;
} }
public void SetValues(in F32Versor versor) public void SetValues(in F32Versor versor)
{ {
this.s0 = versor.GetScalar(); LoadValues(
this.x1 = versor.GetX1(); versor.GetScalar(),
this.x2 = versor.GetX2(); versor.GetX1(),
this.x3 = versor.GetX3(); versor.GetX2(),
versor.GetX3(),
double squareModule = (this.s0 * this.s0 + this.x1 * this.x1) + (this.x2 * this.x2 + this.x3 * this.x3); out this);
this.corrector = 2.0 - squareModule;
if (squareModule < 1.0 - F64Utility.TWO_EPSYLON || 1.0 + F64Utility.TWO_EPSYLON < squareModule)
{
this.Normalize(squareModule);
}
} }
public void SetInverted(in F64Versor versor) public void SetInverted(in F64Versor versor)
@ -264,32 +218,23 @@ namespace BGC
this.x1 = -versor.x1; this.x1 = -versor.x1;
this.x2 = -versor.x2; this.x2 = -versor.x2;
this.x3 = -versor.x3; this.x3 = -versor.x3;
this.corrector = versor.corrector;
} }
public void SetInverted(in F32Versor versor) public void SetInverted(in F32Versor versor)
{ {
this.s0 = versor.GetScalar(); LoadValues(
this.x1 = -versor.GetX1(); versor.GetScalar(),
this.x2 = -versor.GetX2(); versor.GetX1(),
this.x3 = -versor.GetX3(); versor.GetX2(),
versor.GetX3(),
double squareModule = (this.s0 * this.s0 + this.x1 * this.x1) + (this.x2 * this.x2 + this.x3 * this.x3); out this);
this.corrector = 2.0 - squareModule;
if (squareModule < 1.0 - F64Utility.TWO_EPSYLON || 1.0 + F64Utility.TWO_EPSYLON < squareModule)
{
this.Normalize(squareModule);
}
} }
public readonly void Turn(in F64Vector3 vector, out F64Vector3 result) public readonly void Turn(in F64Vector3 vector, out F64Vector3 result)
{ {
double multiplier = 2.0 * this.corrector; double tx1 = 2.0 * (this.x2 * vector.x3 - this.x3 * vector.x2);
double tx1 = multiplier * (this.x2 * vector.x3 - this.x3 * vector.x2); double tx2 = 2.0 * (this.x3 * vector.x1 - this.x1 * vector.x3);
double tx2 = multiplier * (this.x3 * vector.x1 - this.x1 * vector.x3); double tx3 = 2.0 * (this.x1 * vector.x2 - this.x2 * vector.x1);
double tx3 = multiplier * (this.x1 * vector.x2 - this.x2 * vector.x1);
double x1 = (vector.x1 + tx1 * this.s0) + (this.x2 * tx3 - this.x3 * tx2); 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 x2 = (vector.x2 + tx2 * this.s0) + (this.x3 * tx1 - this.x1 * tx3);
@ -302,10 +247,9 @@ namespace BGC
public readonly void TurnBack(in F64Vector3 vector, out F64Vector3 result) public readonly void TurnBack(in F64Vector3 vector, out F64Vector3 result)
{ {
double multiplier = 2.0 * this.corrector; double tx1 = 2.0 * (this.x2 * vector.x3 - this.x3 * vector.x2);
double tx1 = multiplier * (this.x2 * vector.x3 - this.x3 * vector.x2); double tx2 = 2.0 * (this.x3 * vector.x1 - this.x1 * vector.x3);
double tx2 = multiplier * (this.x3 * vector.x1 - this.x1 * vector.x3); double tx3 = 2.0 * (this.x1 * vector.x2 - this.x2 * vector.x1);
double tx3 = multiplier * (this.x1 * vector.x2 - this.x2 * vector.x1);
double x1 = (vector.x1 - tx1 * this.s0) + (this.x2 * tx3 - this.x3 * tx2); 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 x2 = (vector.x2 - tx2 * this.s0) + (this.x3 * tx1 - this.x1 * tx3);
@ -330,30 +274,47 @@ namespace BGC
this.x1 /= module; this.x1 /= module;
this.x2 /= module; this.x2 /= module;
this.x3 /= module; this.x3 /= module;
this.corrector = 2.0 - (this.s0 * this.s0 + this.x1 * this.x1) - (this.x2 * this.x2 + this.x3 * this.x3);
} }
public static void Combine(in F64Versor second, in F64Versor first, out F64Versor result) public static void Combine(in F64Versor second, in F64Versor first, out F64Versor result)
{ {
double s0 = (second.s0 * first.s0 - second.x1 * first.x1) - (second.x2 * first.x2 + second.x3 * first.x3); LoadValues(
double x1 = (second.x1 * first.s0 + second.s0 * first.x1) - (second.x3 * first.x2 - second.x2 * first.x3); (second.s0 * first.s0 - second.x1 * first.x1) - (second.x2 * first.x2 + second.x3 * first.x3),
double x2 = (second.x2 * first.s0 + second.s0 * first.x2) - (second.x1 * first.x3 - second.x3 * first.x1); (second.x1 * first.s0 + second.s0 * first.x1) - (second.x3 * first.x2 - second.x2 * first.x3),
double x3 = (second.x3 * first.s0 + second.s0 * first.x3) - (second.x2 * first.x1 - second.x1 * first.x2); (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),
result.s0 = s0; out result);
result.x1 = x1; }
result.x2 = x2;
result.x3 = x3; public static void LoadIdle(out F64Versor 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 F64Versor versor)
{
versor.s0 = s0;
versor.x1 = x1;
versor.x2 = x2;
versor.x3 = x3;
double squareModule = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3); double squareModule = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3);
result.corrector = 2.0 - squareModule; if (1.0 - F64Utility.TWO_EPSYLON <= squareModule && squareModule <= 1.0 + F64Utility.TWO_EPSYLON)
if (squareModule < 1.0 - F64Utility.TWO_EPSYLON || 1.0 + F64Utility.TWO_EPSYLON < squareModule)
{ {
result.Normalize(squareModule); if (-1.0 + F64Utility.EPSYLON < s0)
{
return;
}
versor.Reset();
return;
} }
versor.Normalize(squareModule);
} }
} }
} }

4
BGCDev/Program.cs
View file

@ -17,7 +17,7 @@ public static class Program
for (int i = 0; i < amount; i++) for (int i = 0; i < amount; i++)
{ {
versors[i] = F32Versor.IDLE; versors[i].Reset();
} }
return versors; return versors;
@ -44,7 +44,7 @@ public static class Program
private static void PrintVersor(in F32Versor versor) private static void PrintVersor(in F32Versor versor)
{ {
Console.WriteLine("({0}, {1}, {2}, {3}) / {4:E}", versor.GetScalar(), versor.GetX1(), versor.GetX2(), versor.GetX3(), versor.GetCorrector() - 1.0f); Console.WriteLine("({0}, {1}, {2}, {3})", versor.GetScalar(), versor.GetX1(), versor.GetX2(), versor.GetX3());
} }
public static int Main() public static int Main()