312 lines
9.6 KiB
C#
312 lines
9.6 KiB
C#
/*
|
|
* 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 BasicGeometry
|
|
{
|
|
public struct FP64Versor
|
|
{
|
|
private double s0 = 1.0;
|
|
private double x1 = 0.0;
|
|
private double x2 = 0.0;
|
|
private double x3 = 0.0;
|
|
|
|
public FP64Versor(double s0, double x1, double x2, double x3)
|
|
{
|
|
LoadValues(s0, x1, x2, x3, out this);
|
|
}
|
|
|
|
public FP64Versor(in FP64Versor versor)
|
|
{
|
|
this.s0 = versor.s0;
|
|
this.x1 = versor.x1;
|
|
this.x2 = versor.x2;
|
|
this.x3 = versor.x3;
|
|
}
|
|
|
|
public FP64Versor(in FP32Versor versor)
|
|
{
|
|
LoadValues(
|
|
versor.GetScalar(),
|
|
versor.GetX1(),
|
|
versor.GetX2(),
|
|
versor.GetX3(),
|
|
out this);
|
|
}
|
|
|
|
public readonly double GetScalar()
|
|
{
|
|
return this.s0;
|
|
}
|
|
|
|
public readonly double GetX1()
|
|
{
|
|
return this.x1;
|
|
}
|
|
|
|
public readonly double GetX2()
|
|
{
|
|
return this.x2;
|
|
}
|
|
|
|
public readonly double GetX3()
|
|
{
|
|
return this.x3;
|
|
}
|
|
|
|
public readonly bool IsIdle()
|
|
{
|
|
return this.s0 <= -(1.0 - FP64Utility.EPSYLON) || (1.0 - FP64Utility.EPSYLON) <= this.s0;
|
|
}
|
|
|
|
public void Reset()
|
|
{
|
|
this.s0 = 1.0;
|
|
this.x1 = 0.0;
|
|
this.x2 = 0.0;
|
|
this.x3 = 0.0;
|
|
}
|
|
|
|
public void Invert()
|
|
{
|
|
this.x1 = -this.x1;
|
|
this.x2 = -this.x2;
|
|
this.x3 = -this.x3;
|
|
}
|
|
|
|
public readonly double GetAngle(AngleUnit unit)
|
|
{
|
|
if (this.s0 <= -(1.0 - FP64Utility.TWO_EPSYLON) || 1.0 - FP64Utility.TWO_EPSYLON <= this.s0) {
|
|
return 0.0;
|
|
}
|
|
|
|
if (-FP64Utility.EPSYLON <= this.s0 && this.s0 <= FP64Utility.EPSYLON)
|
|
{
|
|
return FP64Angle.GetHalfCircle(unit);
|
|
}
|
|
|
|
return FP64Radians.ToUnits(2.0 * Math.Acos(this.s0), unit);
|
|
}
|
|
|
|
public readonly void MakeRotationMatrix(out FP64Matrix3x3 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 s0x1 = 2.0 * this.s0 * this.x1;
|
|
double s0x2 = 2.0 * this.s0 * this.x2;
|
|
double s0x3 = 2.0 * this.s0 * this.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;
|
|
|
|
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 FP64Matrix3x3 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 s0x1 = 2.0 * this.s0 * this.x1;
|
|
double s0x2 = 2.0 * this.s0 * this.x2;
|
|
double s0x3 = 2.0 * this.s0 * this.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;
|
|
|
|
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(double s0, double x1, double x2, double x3)
|
|
{
|
|
this.s0 = s0;
|
|
this.x1 = x1;
|
|
this.x2 = x2;
|
|
this.x3 = x3;
|
|
|
|
double squareModule = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3);
|
|
|
|
if (1.0 - FP64Utility.TWO_EPSYLON <= squareModule && squareModule <= 1.0 + FP64Utility.TWO_EPSYLON)
|
|
{
|
|
return;
|
|
}
|
|
|
|
this.Normalize(squareModule);
|
|
}
|
|
|
|
public void SetValues(in FP64Versor versor)
|
|
{
|
|
this.s0 = versor.s0;
|
|
this.x1 = versor.x1;
|
|
this.x2 = versor.x2;
|
|
this.x3 = versor.x3;
|
|
}
|
|
|
|
public void SetValues(in FP32Versor versor)
|
|
{
|
|
LoadValues(
|
|
versor.GetScalar(),
|
|
versor.GetX1(),
|
|
versor.GetX2(),
|
|
versor.GetX3(),
|
|
out this);
|
|
}
|
|
|
|
public void SetInverted(in FP64Versor versor)
|
|
{
|
|
this.s0 = versor.s0;
|
|
this.x1 = -versor.x1;
|
|
this.x2 = -versor.x2;
|
|
this.x3 = -versor.x3;
|
|
}
|
|
|
|
public void SetInverted(in FP32Versor versor)
|
|
{
|
|
LoadValues(
|
|
versor.GetScalar(),
|
|
versor.GetX1(),
|
|
versor.GetX2(),
|
|
versor.GetX3(),
|
|
out this);
|
|
}
|
|
|
|
public readonly void Turn(in FP64Vector3 vector, out FP64Vector3 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);
|
|
|
|
result.x1 = x1;
|
|
result.x2 = x2;
|
|
result.x3 = x3;
|
|
}
|
|
|
|
public readonly void TurnBack(in FP64Vector3 vector, out FP64Vector3 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);
|
|
|
|
result.x1 = x1;
|
|
result.x2 = x2;
|
|
result.x3 = x3;
|
|
}
|
|
|
|
private void Normalize(double squareModule)
|
|
{
|
|
if (squareModule <= FP64Utility.SQUARE_EPSYLON || (this.x1 * this.x1 + this.x2 * this.x2 + this.x3 * this.x3) <= FP64Utility.SQUARE_EPSYLON * squareModule)
|
|
{
|
|
this.Reset();
|
|
return;
|
|
}
|
|
|
|
double multiplier = Math.Sqrt(1.0 / squareModule);
|
|
|
|
this.s0 *= multiplier;
|
|
this.x1 *= multiplier;
|
|
this.x2 *= multiplier;
|
|
this.x3 *= multiplier;
|
|
}
|
|
|
|
public static void Combine(in FP64Versor second, in FP64Versor first, out FP64Versor 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);
|
|
|
|
result.s0 = s0;
|
|
result.x1 = x1;
|
|
result.x2 = x2;
|
|
result.x3 = x3;
|
|
|
|
if (1.0 - FP64Utility.TWO_EPSYLON <= squareModule && squareModule <= 1.0 + FP64Utility.TWO_EPSYLON)
|
|
{
|
|
return;
|
|
}
|
|
|
|
result.Normalize(squareModule);
|
|
}
|
|
|
|
public static void LoadIdle(out FP64Versor 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 FP64Versor versor)
|
|
{
|
|
versor.s0 = s0;
|
|
versor.x1 = x1;
|
|
versor.x2 = x2;
|
|
versor.x3 = x3;
|
|
|
|
double squareModule = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3);
|
|
|
|
if (1.0 - FP64Utility.TWO_EPSYLON <= squareModule && squareModule <= 1.0 + FP64Utility.TWO_EPSYLON)
|
|
{
|
|
return;
|
|
}
|
|
|
|
versor.Normalize(squareModule);
|
|
}
|
|
}
|
|
}
|