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

Большая реорганизация проекта

Browse source
This commit is contained in:
Andrey Pokidov 2025-02-11 19:38:07 +07:00
parent 233c027d46
commit f3b0232dd4
35 changed files with 705 additions and 593 deletions
Download patch file Download diff file Expand all files Collapse all files

214
BasicGeometry/QuaternionFP64.cs Normal file
View file

@ -0,0 +1,214 @@
using System;
using System.Numerics;
namespace BasicGeometry
{
public struct QuaternionFP64
{
public double s0 = 0.0, x1 = 0.0, x2 = 0.0, x3 = 0.0;
public QuaternionFP64(double s0, double x1, double x2, double x3)
{
this.s0 = s0;
this.x1 = x1;
this.x2 = x2;
this.x3 = x3;
}
public QuaternionFP64(in QuaternionFP32 quaternion)
{
this.s0 = quaternion.s0;
this.x1 = quaternion.x1;
this.x2 = quaternion.x2;
this.x3 = quaternion.x3;
}
public QuaternionFP64(in QuaternionFP64 quaternion)
{
this.s0 = quaternion.s0;
this.x1 = quaternion.x1;
this.x2 = quaternion.x2;
this.x3 = quaternion.x3;
}
public readonly double GetSquareModule()
{
return this.s0 * this.s0 + this.x1 * this.x1 + (this.x2 * this.x2 + this.x3 * this.x3);
}
public readonly double GetModule()
{
return Math.Sqrt(this.GetSquareModule());
}
public readonly bool IsZero()
{
return this.GetSquareModule() <= UtilityFP64.SQUARE_EPSYLON;
}
public readonly bool IsUnit()
{
return UtilityFP64.IsSqareValueUnit(this.GetSquareModule());
}
public void Reset()
{
this.s0 = 0.0;
this.x1 = 0.0;
this.x2 = 0.0;
this.x3 = 0.0;
}
public void SetToIdentity()
{
this.s0 = 1.0;
this.x1 = 0.0;
this.x2 = 0.0;
this.x3 = 0.0;
}
public void Conjugate()
{
this.x1 = -this.x1;
this.x2 = -this.x2;
this.x3 = -this.x3;
}
public void SetValues(double s0, double x1, double x2, double x3)
{
this.s0 = s0;
this.x1 = x1;
this.x2 = x2;
this.x3 = x3;
}
public void SetValues(in QuaternionFP32 quaternion)
{
this.s0 = quaternion.s0;
this.x1 = quaternion.x1;
this.x2 = quaternion.x2;
this.x3 = quaternion.x3;
}
public void SetValues(in QuaternionFP64 quaternion)
{
this.s0 = quaternion.s0;
this.x1 = quaternion.x1;
this.x2 = quaternion.x2;
this.x3 = quaternion.x3;
}
public void SetConjugateOf(in QuaternionFP64 quaternion)
{
this.s0 = quaternion.s0;
this.x1 = -quaternion.x1;
this.x2 = -quaternion.x2;
this.x3 = -quaternion.x3;
}
public readonly void MakeRotationMatrix(out Matrix3x3FP64 matrix)
{
double s0s0 = this.s0 * this.s0;
double x1x1 = this.x1 * this.x1;
double x2x2 = this.x2 * this.x2;
double x3x3 = this.x3 * this.x3;
double squareModule = (s0s0 + x1x1) + (x2x2 + x3x3);
if (squareModule <= UtilityFP64.SQUARE_EPSYLON)
{
Matrix3x3FP64.LoadIdentity(out matrix);
return;
}
double corrector1 = 1.0 / squareModule;
double corrector2 = 2.0 * corrector1;
double s0x1 = this.s0 * this.x1;
double s0x2 = this.s0 * this.x2;
double s0x3 = this.s0 * this.x3;
double x1x2 = this.x1 * this.x2;
double x1x3 = this.x1 * this.x3;
double x2x3 = this.x2 * this.x3;
matrix.r1c1 = corrector1 * ((s0s0 + x1x1) - (x2x2 + x3x3));
matrix.r2c2 = corrector1 * ((s0s0 + x2x2) - (x1x1 + x3x3));
matrix.r3c3 = corrector1 * ((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 Matrix3x3FP64 matrix)
{
double s0s0 = this.s0 * this.s0;
double x1x1 = this.x1 * this.x1;
double x2x2 = this.x2 * this.x2;
double x3x3 = this.x3 * this.x3;
double squareModule = (s0s0 + x1x1) + (x2x2 + x3x3);
if (squareModule <= UtilityFP64.SQUARE_EPSYLON)
{
Matrix3x3FP64.LoadIdentity(out matrix);
return;
}
double corrector1 = 1.0 / squareModule;
double corrector2 = 2.0 * corrector1;
double s0x1 = this.s0 * this.x1;
double s0x2 = this.s0 * this.x2;
double s0x3 = this.s0 * this.x3;
double x1x2 = this.x1 * this.x2;
double x1x3 = this.x1 * this.x3;
double x2x3 = this.x2 * this.x3;
matrix.r1c1 = corrector1 * ((s0s0 + x1x1) - (x2x2 + x3x3));
matrix.r2c2 = corrector1 * ((s0s0 + x2x2) - (x1x1 + x3x3));
matrix.r3c3 = corrector1 * ((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 static void Add(in QuaternionFP64 quaternion1, in QuaternionFP64 quaternion2, out QuaternionFP64 sum)
{
sum.s0 = quaternion1.s0 + quaternion2.s0;
sum.x1 = quaternion1.x1 + quaternion2.x1;
sum.x2 = quaternion1.x2 + quaternion2.x2;
sum.x3 = quaternion1.x3 + quaternion2.x3;
}
public static void Subtract(in QuaternionFP64 minuend, in QuaternionFP64 subtrahend, out QuaternionFP64 difference)
{
difference.s0 = minuend.s0 - subtrahend.s0;
difference.x1 = minuend.x1 - subtrahend.x1;
difference.x2 = minuend.x2 - subtrahend.x2;
difference.x3 = minuend.x3 - subtrahend.x3;
}
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);
product.s0 = s0;
product.x1 = x1;
product.x2 = x2;
product.x3 = x3;
}
}
}