Исправления в матрицах 2x3 и 3x2 / Several fixes in 2x3 and 3x2 matrixes
This commit is contained in:
Andrey Pokidov
parent
3ba55c7524
commit
301cabe8de
22 changed files with 3805 additions and 3744 deletions
|
|
@ -1,201 +1,201 @@
|
|||
using System;
|
||||
using System.Numerics;
|
||||
|
||||
namespace Geometry
|
||||
{
|
||||
public struct DPQuaternion
|
||||
{
|
||||
public double s0, x1, x2, x3;
|
||||
|
||||
public DPQuaternion(double s0, double x1, double x2, double x3)
|
||||
{
|
||||
this.s0 = s0;
|
||||
this.x1 = x1;
|
||||
this.x2 = x2;
|
||||
this.x3 = x3;
|
||||
}
|
||||
|
||||
public DPQuaternion(in SPQuaternion quaternion)
|
||||
{
|
||||
this.s0 = quaternion.s0;
|
||||
this.x1 = quaternion.x1;
|
||||
this.x2 = quaternion.x2;
|
||||
this.x3 = quaternion.x3;
|
||||
}
|
||||
|
||||
public DPQuaternion(in DPQuaternion quaternion)
|
||||
{
|
||||
this.s0 = quaternion.s0;
|
||||
this.x1 = quaternion.x1;
|
||||
this.x2 = quaternion.x2;
|
||||
this.x3 = quaternion.x3;
|
||||
}
|
||||
|
||||
public void Reset()
|
||||
{
|
||||
this.s0 = 0.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 readonly DPQuaternion GetConjugate()
|
||||
{
|
||||
return new DPQuaternion(this.s0, -this.x1, -this.x2, -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 SPQuaternion quaternion)
|
||||
{
|
||||
this.s0 = quaternion.s0;
|
||||
this.x1 = quaternion.x1;
|
||||
this.x2 = quaternion.x2;
|
||||
this.x3 = quaternion.x3;
|
||||
}
|
||||
|
||||
public void SetValues(in DPQuaternion quaternion)
|
||||
{
|
||||
this.s0 = quaternion.s0;
|
||||
this.x1 = quaternion.x1;
|
||||
this.x2 = quaternion.x2;
|
||||
this.x3 = quaternion.x3;
|
||||
}
|
||||
|
||||
public readonly void MakeRotationMatrix(out DPMatrix3x3 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 (-DPUtility.EPSYLON <= squareModule && squareModule <= DPUtility.EPSYLON)
|
||||
{
|
||||
DPMatrix3x3.LoadIdentity(out matrix);
|
||||
return;
|
||||
}
|
||||
|
||||
double corrector1;
|
||||
double corrector2;
|
||||
|
||||
if (1.0 - DPUtility.TWO_EPSYLON <= squareModule && squareModule <= 1.0 + DPUtility.TWO_EPSYLON) {
|
||||
corrector1 = 2.0 - squareModule;
|
||||
corrector2 = 2.0 * corrector1;
|
||||
}
|
||||
else {
|
||||
corrector1 = 1.0 / squareModule;
|
||||
corrector2 = 2.0 / squareModule;
|
||||
}
|
||||
|
||||
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 DPMatrix3x3 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 (-DPUtility.EPSYLON <= squareModule && squareModule <= DPUtility.EPSYLON)
|
||||
{
|
||||
DPMatrix3x3.LoadIdentity(out matrix);
|
||||
return;
|
||||
}
|
||||
|
||||
double corrector1;
|
||||
double corrector2;
|
||||
|
||||
if (1.0 - DPUtility.TWO_EPSYLON <= squareModule && squareModule <= 1.0 + DPUtility.TWO_EPSYLON) {
|
||||
corrector1 = 2.0 - squareModule;
|
||||
corrector2 = 2.0 * corrector1;
|
||||
}
|
||||
else {
|
||||
corrector1 = 1.0 / squareModule;
|
||||
corrector2 = 2.0 / squareModule;
|
||||
}
|
||||
|
||||
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 DPQuaternion quaternion1, in DPQuaternion quaternion2, out DPQuaternion 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 DPQuaternion minuend, in DPQuaternion subtrahend, out DPQuaternion 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 DPQuaternion left, in DPQuaternion right, out DPQuaternion 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;
|
||||
}
|
||||
}
|
||||
}
|
||||
using System;
|
||||
using System.Numerics;
|
||||
|
||||
namespace Geometry
|
||||
{
|
||||
public struct DPQuaternion
|
||||
{
|
||||
public double s0, x1, x2, x3;
|
||||
|
||||
public DPQuaternion(double s0, double x1, double x2, double x3)
|
||||
{
|
||||
this.s0 = s0;
|
||||
this.x1 = x1;
|
||||
this.x2 = x2;
|
||||
this.x3 = x3;
|
||||
}
|
||||
|
||||
public DPQuaternion(in SPQuaternion quaternion)
|
||||
{
|
||||
this.s0 = quaternion.s0;
|
||||
this.x1 = quaternion.x1;
|
||||
this.x2 = quaternion.x2;
|
||||
this.x3 = quaternion.x3;
|
||||
}
|
||||
|
||||
public DPQuaternion(in DPQuaternion quaternion)
|
||||
{
|
||||
this.s0 = quaternion.s0;
|
||||
this.x1 = quaternion.x1;
|
||||
this.x2 = quaternion.x2;
|
||||
this.x3 = quaternion.x3;
|
||||
}
|
||||
|
||||
public void Reset()
|
||||
{
|
||||
this.s0 = 0.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 readonly DPQuaternion GetConjugate()
|
||||
{
|
||||
return new DPQuaternion(this.s0, -this.x1, -this.x2, -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 SPQuaternion quaternion)
|
||||
{
|
||||
this.s0 = quaternion.s0;
|
||||
this.x1 = quaternion.x1;
|
||||
this.x2 = quaternion.x2;
|
||||
this.x3 = quaternion.x3;
|
||||
}
|
||||
|
||||
public void SetValues(in DPQuaternion quaternion)
|
||||
{
|
||||
this.s0 = quaternion.s0;
|
||||
this.x1 = quaternion.x1;
|
||||
this.x2 = quaternion.x2;
|
||||
this.x3 = quaternion.x3;
|
||||
}
|
||||
|
||||
public readonly void MakeRotationMatrix(out DPMatrix3x3 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 (-DPUtility.EPSYLON <= squareModule && squareModule <= DPUtility.EPSYLON)
|
||||
{
|
||||
DPMatrix3x3.LoadIdentity(out matrix);
|
||||
return;
|
||||
}
|
||||
|
||||
double corrector1;
|
||||
double corrector2;
|
||||
|
||||
if (1.0 - DPUtility.TWO_EPSYLON <= squareModule && squareModule <= 1.0 + DPUtility.TWO_EPSYLON) {
|
||||
corrector1 = 2.0 - squareModule;
|
||||
corrector2 = 2.0 * corrector1;
|
||||
}
|
||||
else {
|
||||
corrector1 = 1.0 / squareModule;
|
||||
corrector2 = 2.0 / squareModule;
|
||||
}
|
||||
|
||||
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 DPMatrix3x3 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 (-DPUtility.EPSYLON <= squareModule && squareModule <= DPUtility.EPSYLON)
|
||||
{
|
||||
DPMatrix3x3.LoadIdentity(out matrix);
|
||||
return;
|
||||
}
|
||||
|
||||
double corrector1;
|
||||
double corrector2;
|
||||
|
||||
if (1.0 - DPUtility.TWO_EPSYLON <= squareModule && squareModule <= 1.0 + DPUtility.TWO_EPSYLON) {
|
||||
corrector1 = 2.0 - squareModule;
|
||||
corrector2 = 2.0 * corrector1;
|
||||
}
|
||||
else {
|
||||
corrector1 = 1.0 / squareModule;
|
||||
corrector2 = 2.0 / squareModule;
|
||||
}
|
||||
|
||||
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 DPQuaternion quaternion1, in DPQuaternion quaternion2, out DPQuaternion 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 DPQuaternion minuend, in DPQuaternion subtrahend, out DPQuaternion 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 DPQuaternion left, in DPQuaternion right, out DPQuaternion 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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue