Большое переименование и рефакторинг углов / Big renaming and refactoring of angles
This commit is contained in:
parent
1afbf8c1d6
commit
72d06a23b2
38 changed files with 3819 additions and 3757 deletions
366
BGC/F32Versor.cs
Normal file
366
BGC/F32Versor.cs
Normal file
|
|
@ -0,0 +1,366 @@
|
|||
/*
|
||||
* Copyright 2019-2025 Andrey Pokidov <andrey.pokidov@gmail.com>
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
/*
|
||||
* Author: Andrey Pokidov
|
||||
* Date: 20 Oct 2024
|
||||
*/
|
||||
|
||||
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()
|
||||
{
|
||||
this.s0 = 1.0f;
|
||||
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();
|
||||
}
|
||||
}
|
||||
|
||||
public F32Versor(in F32Versor versor)
|
||||
{
|
||||
this.s0 = versor.s0;
|
||||
this.x1 = versor.x1;
|
||||
this.x2 = versor.x2;
|
||||
this.x3 = versor.x3;
|
||||
this.corrector = versor.corrector;
|
||||
}
|
||||
|
||||
public F32Versor(in F64Versor versor)
|
||||
{
|
||||
this.s0 = (float)versor.GetScalar();
|
||||
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()
|
||||
{
|
||||
return this.s0;
|
||||
}
|
||||
|
||||
public readonly float GetX1()
|
||||
{
|
||||
return this.x1;
|
||||
}
|
||||
|
||||
public readonly float GetX2()
|
||||
{
|
||||
return this.x2;
|
||||
}
|
||||
|
||||
public readonly float GetX3()
|
||||
{
|
||||
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;
|
||||
}
|
||||
|
||||
public void Reset()
|
||||
{
|
||||
this.s0 = 1.0f;
|
||||
this.x1 = 0.0f;
|
||||
this.x2 = 0.0f;
|
||||
this.x3 = 0.0f;
|
||||
this.corrector = 1.0f;
|
||||
}
|
||||
|
||||
public void Invert()
|
||||
{
|
||||
this.x1 = -this.x1;
|
||||
this.x2 = -this.x2;
|
||||
this.x3 = -this.x3;
|
||||
}
|
||||
|
||||
public readonly float GetAngle(AngleUnit unit)
|
||||
{
|
||||
if (this.s0 <= -(1.0f - F32Utility.TWO_EPSYLON) || 1.0f - F32Utility.TWO_EPSYLON <= this.s0) {
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
if (-F32Utility.EPSYLON <= this.s0 && this.s0 <= F32Utility.EPSYLON)
|
||||
{
|
||||
return F32Angle.GetHalfCircle(unit);
|
||||
}
|
||||
|
||||
return F32Radians.ToUnits(2.0f * MathF.Acos(s0), unit);
|
||||
}
|
||||
|
||||
public readonly void MakeRotationMatrix(out F32Matrix3x3 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 = this.s0 * this.x1;
|
||||
float s0x2 = this.s0 * this.x2;
|
||||
float s0x3 = this.s0 * this.x3;
|
||||
|
||||
float x1x2 = this.x1 * this.x2;
|
||||
float x1x3 = this.x1 * this.x3;
|
||||
float x2x3 = this.x2 * this.x3;
|
||||
|
||||
float corrector2 = 2.0f * this.corrector;
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
public readonly void MakeReverseMatrix(out F32Matrix3x3 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 = this.s0 * this.x1;
|
||||
float s0x2 = this.s0 * this.x2;
|
||||
float s0x3 = this.s0 * this.x3;
|
||||
|
||||
float x1x2 = this.x1 * this.x2;
|
||||
float x1x3 = this.x1 * this.x3;
|
||||
float x2x3 = this.x2 * this.x3;
|
||||
|
||||
float corrector2 = 2.0f * this.corrector;
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
public void SetValues(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);
|
||||
|
||||
if (squareModule < 1.0f - F32Utility.TWO_EPSYLON || 1.0f + F32Utility.TWO_EPSYLON < squareModule)
|
||||
{
|
||||
this.Normalize(squareModule);
|
||||
return;
|
||||
}
|
||||
|
||||
if (s0 < -1.0f + F32Utility.EPSYLON || 1.0f - F32Utility.EPSYLON < s0) {
|
||||
this.Reset();
|
||||
return;
|
||||
}
|
||||
|
||||
this.corrector = 2.0f - squareModule;
|
||||
}
|
||||
|
||||
public void SetValues(in F32Versor versor)
|
||||
{
|
||||
this.s0 = versor.s0;
|
||||
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()
|
||||
);
|
||||
}
|
||||
|
||||
public void SetInverted(in F32Versor versor)
|
||||
{
|
||||
this.s0 = versor.s0;
|
||||
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()
|
||||
);
|
||||
}
|
||||
|
||||
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 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 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 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 <= F32Utility.SQUARE_EPSYLON || (this.x1 * this.x1 + this.x2 * this.x2 + this.x3 * this.x3) <= F32Utility.SQUARE_EPSYLON * squareModule)
|
||||
{
|
||||
this.Reset();
|
||||
return;
|
||||
}
|
||||
|
||||
float module = MathF.Sqrt(squareModule);
|
||||
|
||||
this.s0 /= module;
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
}
|
||||
}
|
||||
Loading…
Add table
Add a link
Reference in a new issue