mirror of
https://github.com/Ryujinx/Opentk.git
synced 2025-01-22 23:01:06 +00:00
Added JTalton's double precision mathlib.
This commit is contained in:
the_fiddler
parent
0911cf3072
commit
d4a63df42b
|
|
@ -218,21 +218,6 @@ namespace Bind.Structures
|
||||||
|
|
||||||
if (Parameters.HasPointerParameters)
|
if (Parameters.HasPointerParameters)
|
||||||
{
|
{
|
||||||
// Pointer overloads
|
|
||||||
foreach (Parameter p in this.Parameters)
|
|
||||||
{
|
|
||||||
if (p.WrapperType == WrapperTypes.ArrayParameter)
|
|
||||||
{
|
|
||||||
p.Reference = false;
|
|
||||||
p.Array = 0;
|
|
||||||
p.Pointer = true;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
f = new Function(this);
|
|
||||||
f.CreateBody(false);
|
|
||||||
wrappers.Add(f);
|
|
||||||
new Function(f).WrapVoidPointers(wrappers);
|
|
||||||
|
|
||||||
// Array overloads
|
// Array overloads
|
||||||
foreach (Parameter p in this.Parameters)
|
foreach (Parameter p in this.Parameters)
|
||||||
{
|
{
|
||||||
|
|
@ -262,6 +247,23 @@ namespace Bind.Structures
|
||||||
f.CreateBody(false);
|
f.CreateBody(false);
|
||||||
wrappers.Add(f);
|
wrappers.Add(f);
|
||||||
new Function(f).WrapVoidPointers(wrappers);
|
new Function(f).WrapVoidPointers(wrappers);
|
||||||
|
|
||||||
|
// Pointer overloads
|
||||||
|
// Should be last to work around Intellisense bug, where
|
||||||
|
// array overloads are not reported if there is a pointer overload.
|
||||||
|
foreach (Parameter p in this.Parameters)
|
||||||
|
{
|
||||||
|
if (p.WrapperType == WrapperTypes.ArrayParameter)
|
||||||
|
{
|
||||||
|
p.Reference = false;
|
||||||
|
p.Array = 0;
|
||||||
|
p.Pointer = true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
f = new Function(this);
|
||||||
|
f.CreateBody(false);
|
||||||
|
wrappers.Add(f);
|
||||||
|
new Function(f).WrapVoidPointers(wrappers);
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
|
|
|
||||||
791
Source/OpenTK/Math/Matrix3d.cs
Normal file
791
Source/OpenTK/Math/Matrix3d.cs
Normal file
|
|
@ -0,0 +1,791 @@
|
||||||
|
using System;
|
||||||
|
using System.Runtime.InteropServices;
|
||||||
|
|
||||||
|
namespace OpenTK.Math
|
||||||
|
{
|
||||||
|
[Serializable]
|
||||||
|
[StructLayout(LayoutKind.Sequential)]
|
||||||
|
public struct Matrix3d : IEquatable<Matrix3d>
|
||||||
|
{
|
||||||
|
#region Fields & Access
|
||||||
|
|
||||||
|
/// <summary>Row 0, Column 0</summary>
|
||||||
|
public double R0C0;
|
||||||
|
|
||||||
|
/// <summary>Row 0, Column 1</summary>
|
||||||
|
public double R0C1;
|
||||||
|
|
||||||
|
/// <summary>Row 0, Column 2</summary>
|
||||||
|
public double R0C2;
|
||||||
|
|
||||||
|
/// <summary>Row 1, Column 0</summary>
|
||||||
|
public double R1C0;
|
||||||
|
|
||||||
|
/// <summary>Row 1, Column 1</summary>
|
||||||
|
public double R1C1;
|
||||||
|
|
||||||
|
/// <summary>Row 1, Column 2</summary>
|
||||||
|
public double R1C2;
|
||||||
|
|
||||||
|
/// <summary>Row 2, Column 0</summary>
|
||||||
|
public double R2C0;
|
||||||
|
|
||||||
|
/// <summary>Row 2, Column 1</summary>
|
||||||
|
public double R2C1;
|
||||||
|
|
||||||
|
/// <summary>Row 2, Column 2</summary>
|
||||||
|
public double R2C2;
|
||||||
|
|
||||||
|
/// <summary>Gets the component at the given row and column in the matrix.</summary>
|
||||||
|
/// <param name="row">The row of the matrix.</param>
|
||||||
|
/// <param name="column">The column of the matrix.</param>
|
||||||
|
/// <returns>The component at the given row and column in the matrix.</returns>
|
||||||
|
public double this[int row, int column]
|
||||||
|
{
|
||||||
|
get
|
||||||
|
{
|
||||||
|
switch( row )
|
||||||
|
{
|
||||||
|
case 0:
|
||||||
|
switch (column)
|
||||||
|
{
|
||||||
|
case 0: return R0C0;
|
||||||
|
case 1: return R0C1;
|
||||||
|
case 2: return R0C2;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case 1:
|
||||||
|
switch (column)
|
||||||
|
{
|
||||||
|
case 0: return R1C0;
|
||||||
|
case 1: return R1C1;
|
||||||
|
case 2: return R1C2;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case 2:
|
||||||
|
switch (column)
|
||||||
|
{
|
||||||
|
case 0: return R2C0;
|
||||||
|
case 1: return R2C1;
|
||||||
|
case 2: return R2C2;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
throw new IndexOutOfRangeException();
|
||||||
|
}
|
||||||
|
set
|
||||||
|
{
|
||||||
|
switch( row )
|
||||||
|
{
|
||||||
|
case 0:
|
||||||
|
switch (column)
|
||||||
|
{
|
||||||
|
case 0: R0C0 = value; return;
|
||||||
|
case 1: R0C1 = value; return;
|
||||||
|
case 2: R0C2 = value; return;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case 1:
|
||||||
|
switch (column)
|
||||||
|
{
|
||||||
|
case 0: R1C0 = value; return;
|
||||||
|
case 1: R1C1 = value; return;
|
||||||
|
case 2: R1C2 = value; return;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case 2:
|
||||||
|
switch (column)
|
||||||
|
{
|
||||||
|
case 0: R2C0 = value; return;
|
||||||
|
case 1: R2C1 = value; return;
|
||||||
|
case 2: R2C2 = value; return;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
throw new IndexOutOfRangeException();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>Gets the component at the index into the matrix.</summary>
|
||||||
|
/// <param name="index">The index into the components of the matrix.</param>
|
||||||
|
/// <returns>The component at the given index into the matrix.</returns>
|
||||||
|
public double this[int index]
|
||||||
|
{
|
||||||
|
get
|
||||||
|
{
|
||||||
|
switch (index)
|
||||||
|
{
|
||||||
|
case 0: return R0C0;
|
||||||
|
case 1: return R0C1;
|
||||||
|
case 2: return R0C2;
|
||||||
|
case 3: return R1C0;
|
||||||
|
case 4: return R1C1;
|
||||||
|
case 5: return R1C2;
|
||||||
|
case 6: return R2C0;
|
||||||
|
case 7: return R2C1;
|
||||||
|
case 8: return R2C2;
|
||||||
|
default: throw new IndexOutOfRangeException();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
set
|
||||||
|
{
|
||||||
|
switch (index)
|
||||||
|
{
|
||||||
|
case 0: R0C0 = value; return;
|
||||||
|
case 1: R0C1 = value; return;
|
||||||
|
case 2: R0C2 = value; return;
|
||||||
|
case 3: R1C0 = value; return;
|
||||||
|
case 4: R1C1 = value; return;
|
||||||
|
case 5: R1C2 = value; return;
|
||||||
|
case 6: R2C0 = value; return;
|
||||||
|
case 7: R2C1 = value; return;
|
||||||
|
case 8: R2C2 = value; return;
|
||||||
|
default: throw new IndexOutOfRangeException();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>Converts the matrix into an IntPtr.</summary>
|
||||||
|
/// <param name="matrix">The matrix to convert.</param>
|
||||||
|
/// <returns>An IntPtr for the matrix.</returns>
|
||||||
|
public static explicit operator IntPtr(Matrix3d matrix)
|
||||||
|
{
|
||||||
|
unsafe
|
||||||
|
{
|
||||||
|
return (IntPtr)(&matrix.R0C0);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>Converts the matrix into left double*.</summary>
|
||||||
|
/// <param name="matrix">The matrix to convert.</param>
|
||||||
|
/// <returns>A double* for the matrix.</returns>
|
||||||
|
[CLSCompliant(false)]
|
||||||
|
unsafe public static explicit operator double*(Matrix3d matrix)
|
||||||
|
{
|
||||||
|
return &matrix.R0C0;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>Converts the matrix into an array of doubles.</summary>
|
||||||
|
/// <param name="matrix">The matrix to convert.</param>
|
||||||
|
/// <returns>An array of doubles for the matrix.</returns>
|
||||||
|
public static explicit operator double[](Matrix3d matrix)
|
||||||
|
{
|
||||||
|
return new double[9]
|
||||||
|
{
|
||||||
|
matrix.R0C0,
|
||||||
|
matrix.R0C1,
|
||||||
|
matrix.R0C2,
|
||||||
|
matrix.R1C0,
|
||||||
|
matrix.R1C1,
|
||||||
|
matrix.R1C2,
|
||||||
|
matrix.R2C0,
|
||||||
|
matrix.R2C1,
|
||||||
|
matrix.R2C2
|
||||||
|
};
|
||||||
|
}
|
||||||
|
|
||||||
|
#endregion
|
||||||
|
|
||||||
|
|
||||||
|
#region Constructors
|
||||||
|
|
||||||
|
/// <summary>Constructs left matrix with the same components as the given matrix.</summary>
|
||||||
|
/// <param name="vector">The matrix whose components to copy.</param>
|
||||||
|
public Matrix3d(ref Matrix3d matrix)
|
||||||
|
{
|
||||||
|
this.R0C0 = matrix.R0C0;
|
||||||
|
this.R0C1 = matrix.R0C1;
|
||||||
|
this.R0C2 = matrix.R0C2;
|
||||||
|
this.R1C0 = matrix.R1C0;
|
||||||
|
this.R1C1 = matrix.R1C1;
|
||||||
|
this.R1C2 = matrix.R1C2;
|
||||||
|
this.R2C0 = matrix.R2C0;
|
||||||
|
this.R2C1 = matrix.R2C1;
|
||||||
|
this.R2C2 = matrix.R2C2;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>Constructs left matrix with the given values.</summary>
|
||||||
|
/// <param name="r0c0">The value for row 0 column 0.</param>
|
||||||
|
/// <param name="r0c1">The value for row 0 column 1.</param>
|
||||||
|
/// <param name="r0c2">The value for row 0 column 2.</param>
|
||||||
|
/// <param name="r1c0">The value for row 1 column 0.</param>
|
||||||
|
/// <param name="r1c1">The value for row 1 column 1.</param>
|
||||||
|
/// <param name="r1c2">The value for row 1 column 2.</param>
|
||||||
|
/// <param name="r2c0">The value for row 2 column 0.</param>
|
||||||
|
/// <param name="r2c1">The value for row 2 column 1.</param>
|
||||||
|
/// <param name="r2c2">The value for row 2 column 2.</param>
|
||||||
|
public Matrix3d
|
||||||
|
(
|
||||||
|
double r0c0,
|
||||||
|
double r0c1,
|
||||||
|
double r0c2,
|
||||||
|
double r1c0,
|
||||||
|
double r1c1,
|
||||||
|
double r1c2,
|
||||||
|
double r2c0,
|
||||||
|
double r2c1,
|
||||||
|
double r2c2
|
||||||
|
)
|
||||||
|
{
|
||||||
|
this.R0C0 = r0c0;
|
||||||
|
this.R0C1 = r0c1;
|
||||||
|
this.R0C2 = r0c2;
|
||||||
|
this.R1C0 = r1c0;
|
||||||
|
this.R1C1 = r1c1;
|
||||||
|
this.R1C2 = r1c2;
|
||||||
|
this.R2C0 = r2c0;
|
||||||
|
this.R2C1 = r2c1;
|
||||||
|
this.R2C2 = r2c2;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>Constructs left matrix from the given array of double-precision floating point numbers.</summary>
|
||||||
|
/// <param name="doubleArray">The array of doubles for the components of the matrix.</param>
|
||||||
|
public Matrix3d(double[] doubleArray)
|
||||||
|
{
|
||||||
|
if (doubleArray == null || doubleArray.GetLength(0) < 9) throw new MissingFieldException();
|
||||||
|
|
||||||
|
this.R0C0 = doubleArray[0];
|
||||||
|
this.R0C1 = doubleArray[1];
|
||||||
|
this.R0C2 = doubleArray[2];
|
||||||
|
this.R1C0 = doubleArray[3];
|
||||||
|
this.R1C1 = doubleArray[4];
|
||||||
|
this.R1C2 = doubleArray[5];
|
||||||
|
this.R2C0 = doubleArray[6];
|
||||||
|
this.R2C1 = doubleArray[7];
|
||||||
|
this.R2C2 = doubleArray[8];
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>Constructs left matrix from the given quaternion.</summary>
|
||||||
|
/// <param name="quaternion">The quaternion to use to construct the martix.</param>
|
||||||
|
public Matrix3d(ref Quaterniond quaternion)
|
||||||
|
{
|
||||||
|
Quaterniond quaternionNormalized;
|
||||||
|
quaternion.Normalize(out quaternionNormalized);
|
||||||
|
|
||||||
|
double xx = quaternionNormalized.X * quaternionNormalized.X;
|
||||||
|
double yy = quaternionNormalized.Y * quaternionNormalized.Y;
|
||||||
|
double zz = quaternionNormalized.Z * quaternionNormalized.Z;
|
||||||
|
double xy = quaternionNormalized.X * quaternionNormalized.Y;
|
||||||
|
double xz = quaternionNormalized.X * quaternionNormalized.Z;
|
||||||
|
double yz = quaternionNormalized.Y * quaternionNormalized.Z;
|
||||||
|
double wx = quaternionNormalized.W * quaternionNormalized.X;
|
||||||
|
double wy = quaternionNormalized.W * quaternionNormalized.Y;
|
||||||
|
double wz = quaternionNormalized.W * quaternionNormalized.Z;
|
||||||
|
|
||||||
|
R0C0 = 1 - 2 * (yy + zz);
|
||||||
|
R0C1 = 2 * (xy - wz);
|
||||||
|
R0C2 = 2 * (xz + wy);
|
||||||
|
|
||||||
|
R1C0 = 2 * (xy + wz);
|
||||||
|
R1C1 = 1 - 2 * (xx + zz);
|
||||||
|
R1C2 = 2 * (yz - wx);
|
||||||
|
|
||||||
|
R2C0 = 2 * (xz - wy);
|
||||||
|
R2C1 = 2 * (yz + wx);
|
||||||
|
R2C2 = 1 - 2 * (xx + yy);
|
||||||
|
}
|
||||||
|
|
||||||
|
#endregion
|
||||||
|
|
||||||
|
|
||||||
|
#region Equality
|
||||||
|
|
||||||
|
/// <summary>Indicates whether the current matrix is equal to another matrix.</summary>
|
||||||
|
/// <param name="matrix">An matrix to compare with this matrix.</param>
|
||||||
|
/// <returns>true if the current matrix is equal to the matrix parameter; otherwise, false.</returns>
|
||||||
|
public bool Equals(Matrix3d matrix)
|
||||||
|
{
|
||||||
|
return
|
||||||
|
R0C0 == matrix.R0C0 &&
|
||||||
|
R0C1 == matrix.R0C1 &&
|
||||||
|
R0C2 == matrix.R0C2 &&
|
||||||
|
R1C0 == matrix.R1C0 &&
|
||||||
|
R1C1 == matrix.R1C1 &&
|
||||||
|
R1C2 == matrix.R1C2 &&
|
||||||
|
R2C0 == matrix.R2C0 &&
|
||||||
|
R2C1 == matrix.R2C1 &&
|
||||||
|
R2C2 == matrix.R2C2;
|
||||||
|
}
|
||||||
|
|
||||||
|
public bool Equals(ref Matrix3d matrix)
|
||||||
|
{
|
||||||
|
return
|
||||||
|
R0C0 == matrix.R0C0 &&
|
||||||
|
R0C1 == matrix.R0C1 &&
|
||||||
|
R0C2 == matrix.R0C2 &&
|
||||||
|
R1C0 == matrix.R1C0 &&
|
||||||
|
R1C1 == matrix.R1C1 &&
|
||||||
|
R1C2 == matrix.R1C2 &&
|
||||||
|
R2C0 == matrix.R2C0 &&
|
||||||
|
R2C1 == matrix.R2C1 &&
|
||||||
|
R2C2 == matrix.R2C2;
|
||||||
|
}
|
||||||
|
|
||||||
|
public static bool Equals(ref Matrix3d left, ref Matrix3d right)
|
||||||
|
{
|
||||||
|
return
|
||||||
|
left.R0C0 == right.R0C0 &&
|
||||||
|
left.R0C1 == right.R0C1 &&
|
||||||
|
left.R0C2 == right.R0C2 &&
|
||||||
|
left.R1C0 == right.R1C0 &&
|
||||||
|
left.R1C1 == right.R1C1 &&
|
||||||
|
left.R1C2 == right.R1C2 &&
|
||||||
|
left.R2C0 == right.R2C0 &&
|
||||||
|
left.R2C1 == right.R2C1 &&
|
||||||
|
left.R2C2 == right.R2C2;
|
||||||
|
}
|
||||||
|
|
||||||
|
public bool EqualsApprox(ref Matrix3d matrix, double tolerance)
|
||||||
|
{
|
||||||
|
return
|
||||||
|
System.Math.Abs(R0C0 - matrix.R0C0) <= tolerance &&
|
||||||
|
System.Math.Abs(R0C1 - matrix.R0C1) <= tolerance &&
|
||||||
|
System.Math.Abs(R0C2 - matrix.R0C2) <= tolerance &&
|
||||||
|
System.Math.Abs(R1C0 - matrix.R1C0) <= tolerance &&
|
||||||
|
System.Math.Abs(R1C1 - matrix.R1C1) <= tolerance &&
|
||||||
|
System.Math.Abs(R1C2 - matrix.R1C2) <= tolerance &&
|
||||||
|
System.Math.Abs(R2C0 - matrix.R2C0) <= tolerance &&
|
||||||
|
System.Math.Abs(R2C1 - matrix.R2C1) <= tolerance &&
|
||||||
|
System.Math.Abs(R2C2 - matrix.R2C2) <= tolerance;
|
||||||
|
}
|
||||||
|
|
||||||
|
public static bool EqualsApprox(ref Matrix3d left, ref Matrix3d right, double tolerance)
|
||||||
|
{
|
||||||
|
return
|
||||||
|
System.Math.Abs(left.R0C0 - right.R0C0) <= tolerance &&
|
||||||
|
System.Math.Abs(left.R0C1 - right.R0C1) <= tolerance &&
|
||||||
|
System.Math.Abs(left.R0C2 - right.R0C2) <= tolerance &&
|
||||||
|
System.Math.Abs(left.R1C0 - right.R1C0) <= tolerance &&
|
||||||
|
System.Math.Abs(left.R1C1 - right.R1C1) <= tolerance &&
|
||||||
|
System.Math.Abs(left.R1C2 - right.R1C2) <= tolerance &&
|
||||||
|
System.Math.Abs(left.R2C0 - right.R2C0) <= tolerance &&
|
||||||
|
System.Math.Abs(left.R2C1 - right.R2C1) <= tolerance &&
|
||||||
|
System.Math.Abs(left.R2C2 - right.R2C2) <= tolerance;
|
||||||
|
}
|
||||||
|
|
||||||
|
#endregion
|
||||||
|
|
||||||
|
|
||||||
|
#region Arithmetic Operators
|
||||||
|
|
||||||
|
|
||||||
|
/// <summary>Add left matrix to this matrix.</summary>
|
||||||
|
/// <param name="matrix">The matrix to add.</param>
|
||||||
|
public void Add(ref Matrix3d matrix)
|
||||||
|
{
|
||||||
|
R0C0 = R0C0 + matrix.R0C0;
|
||||||
|
R0C1 = R0C1 + matrix.R0C1;
|
||||||
|
R0C2 = R0C2 + matrix.R0C2;
|
||||||
|
R1C0 = R1C0 + matrix.R1C0;
|
||||||
|
R1C1 = R1C1 + matrix.R1C1;
|
||||||
|
R1C2 = R1C2 + matrix.R1C2;
|
||||||
|
R2C0 = R2C0 + matrix.R2C0;
|
||||||
|
R2C1 = R2C1 + matrix.R2C1;
|
||||||
|
R2C2 = R2C2 + matrix.R2C2;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>Add left matrix to this matrix.</summary>
|
||||||
|
/// <param name="matrix">The matrix to add.</param>
|
||||||
|
/// <param name="result">The resulting matrix of the addition.</param>
|
||||||
|
public void Add(ref Matrix3d matrix, out Matrix3d result)
|
||||||
|
{
|
||||||
|
result.R0C0 = R0C0 + matrix.R0C0;
|
||||||
|
result.R0C1 = R0C1 + matrix.R0C1;
|
||||||
|
result.R0C2 = R0C2 + matrix.R0C2;
|
||||||
|
result.R1C0 = R1C0 + matrix.R1C0;
|
||||||
|
result.R1C1 = R1C1 + matrix.R1C1;
|
||||||
|
result.R1C2 = R1C2 + matrix.R1C2;
|
||||||
|
result.R2C0 = R2C0 + matrix.R2C0;
|
||||||
|
result.R2C1 = R2C1 + matrix.R2C1;
|
||||||
|
result.R2C2 = R2C2 + matrix.R2C2;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>Add left matrix to left matrix.</summary>
|
||||||
|
/// <param name="matrix">The matrix on the matrix side of the equation.</param>
|
||||||
|
/// <param name="right">The matrix on the right side of the equation</param>
|
||||||
|
/// <param name="result">The resulting matrix of the addition.</param>
|
||||||
|
public static void Add(ref Matrix3d left, ref Matrix3d right, out Matrix3d result)
|
||||||
|
{
|
||||||
|
result.R0C0 = left.R0C0 + right.R0C0;
|
||||||
|
result.R0C1 = left.R0C1 + right.R0C1;
|
||||||
|
result.R0C2 = left.R0C2 + right.R0C2;
|
||||||
|
result.R1C0 = left.R1C0 + right.R1C0;
|
||||||
|
result.R1C1 = left.R1C1 + right.R1C1;
|
||||||
|
result.R1C2 = left.R1C2 + right.R1C2;
|
||||||
|
result.R2C0 = left.R2C0 + right.R2C0;
|
||||||
|
result.R2C1 = left.R2C1 + right.R2C1;
|
||||||
|
result.R2C2 = left.R2C2 + right.R2C2;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
/// <summary>Subtract left matrix from this matrix.</summary>
|
||||||
|
/// <param name="matrix">The matrix to subtract.</param>
|
||||||
|
public void Subtract(ref Matrix3d matrix)
|
||||||
|
{
|
||||||
|
R0C0 = R0C0 + matrix.R0C0;
|
||||||
|
R0C1 = R0C1 + matrix.R0C1;
|
||||||
|
R0C2 = R0C2 + matrix.R0C2;
|
||||||
|
R1C0 = R1C0 + matrix.R1C0;
|
||||||
|
R1C1 = R1C1 + matrix.R1C1;
|
||||||
|
R1C2 = R1C2 + matrix.R1C2;
|
||||||
|
R2C0 = R2C0 + matrix.R2C0;
|
||||||
|
R2C1 = R2C1 + matrix.R2C1;
|
||||||
|
R2C2 = R2C2 + matrix.R2C2;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>Subtract left matrix from this matrix.</summary>
|
||||||
|
/// <param name="matrix">The matrix to subtract.</param>
|
||||||
|
/// <param name="result">The resulting matrix of the subtraction.</param>
|
||||||
|
public void Subtract(ref Matrix3d matrix, out Matrix3d result)
|
||||||
|
{
|
||||||
|
result.R0C0 = R0C0 + matrix.R0C0;
|
||||||
|
result.R0C1 = R0C1 + matrix.R0C1;
|
||||||
|
result.R0C2 = R0C2 + matrix.R0C2;
|
||||||
|
result.R1C0 = R1C0 + matrix.R1C0;
|
||||||
|
result.R1C1 = R1C1 + matrix.R1C1;
|
||||||
|
result.R1C2 = R1C2 + matrix.R1C2;
|
||||||
|
result.R2C0 = R2C0 + matrix.R2C0;
|
||||||
|
result.R2C1 = R2C1 + matrix.R2C1;
|
||||||
|
result.R2C2 = R2C2 + matrix.R2C2;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>Subtract left matrix from left matrix.</summary>
|
||||||
|
/// <param name="matrix">The matrix on the matrix side of the equation.</param>
|
||||||
|
/// <param name="right">The matrix on the right side of the equation</param>
|
||||||
|
/// <param name="result">The resulting matrix of the subtraction.</param>
|
||||||
|
public static void Subtract(ref Matrix3d left, ref Matrix3d right, out Matrix3d result)
|
||||||
|
{
|
||||||
|
result.R0C0 = left.R0C0 + right.R0C0;
|
||||||
|
result.R0C1 = left.R0C1 + right.R0C1;
|
||||||
|
result.R0C2 = left.R0C2 + right.R0C2;
|
||||||
|
result.R1C0 = left.R1C0 + right.R1C0;
|
||||||
|
result.R1C1 = left.R1C1 + right.R1C1;
|
||||||
|
result.R1C2 = left.R1C2 + right.R1C2;
|
||||||
|
result.R2C0 = left.R2C0 + right.R2C0;
|
||||||
|
result.R2C1 = left.R2C1 + right.R2C1;
|
||||||
|
result.R2C2 = left.R2C2 + right.R2C2;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
/// <summary>Multiply left martix times this matrix.</summary>
|
||||||
|
/// <param name="matrix">The matrix to multiply.</param>
|
||||||
|
public void Multiply(ref Matrix3d matrix)
|
||||||
|
{
|
||||||
|
double r0c0 = matrix.R0C0 * R0C0 + matrix.R0C1 * R1C0 + matrix.R0C2 * R2C0;
|
||||||
|
double r0c1 = matrix.R0C0 * R0C1 + matrix.R0C1 * R1C1 + matrix.R0C2 * R2C1;
|
||||||
|
double r0c2 = matrix.R0C0 * R0C2 + matrix.R0C1 * R1C2 + matrix.R0C2 * R2C2;
|
||||||
|
|
||||||
|
double r1c0 = matrix.R1C0 * R0C0 + matrix.R1C1 * R1C0 + matrix.R1C2 * R2C0;
|
||||||
|
double r1c1 = matrix.R1C0 * R0C1 + matrix.R1C1 * R1C1 + matrix.R1C2 * R2C1;
|
||||||
|
double r1c2 = matrix.R1C0 * R0C2 + matrix.R1C1 * R1C2 + matrix.R1C2 * R2C2;
|
||||||
|
|
||||||
|
R2C0 = matrix.R2C0 * R0C0 + matrix.R2C1 * R1C0 + matrix.R2C2 * R2C0;
|
||||||
|
R2C1 = matrix.R2C0 * R0C1 + matrix.R2C1 * R1C1 + matrix.R2C2 * R2C1;
|
||||||
|
R2C2 = matrix.R2C0 * R0C2 + matrix.R2C1 * R1C2 + matrix.R2C2 * R2C2;
|
||||||
|
|
||||||
|
|
||||||
|
R0C0 = r0c0;
|
||||||
|
R0C1 = r0c1;
|
||||||
|
R0C2 = r0c2;
|
||||||
|
|
||||||
|
R1C0 = r1c0;
|
||||||
|
R1C1 = r1c1;
|
||||||
|
R1C2 = r1c2;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>Multiply matrix times this matrix.</summary>
|
||||||
|
/// <param name="matrix">The matrix to multiply.</param>
|
||||||
|
/// <param name="result">The resulting matrix of the multiplication.</param>
|
||||||
|
public void Multiply(ref Matrix3d matrix, out Matrix3d result)
|
||||||
|
{
|
||||||
|
result.R0C0 = matrix.R0C0 * R0C0 + matrix.R0C1 * R1C0 + matrix.R0C2 * R2C0;
|
||||||
|
result.R0C1 = matrix.R0C0 * R0C1 + matrix.R0C1 * R1C1 + matrix.R0C2 * R2C1;
|
||||||
|
result.R0C2 = matrix.R0C0 * R0C2 + matrix.R0C1 * R1C2 + matrix.R0C2 * R2C2;
|
||||||
|
result.R1C0 = matrix.R1C0 * R0C0 + matrix.R1C1 * R1C0 + matrix.R1C2 * R2C0;
|
||||||
|
result.R1C1 = matrix.R1C0 * R0C1 + matrix.R1C1 * R1C1 + matrix.R1C2 * R2C1;
|
||||||
|
result.R1C2 = matrix.R1C0 * R0C2 + matrix.R1C1 * R1C2 + matrix.R1C2 * R2C2;
|
||||||
|
result.R2C0 = matrix.R2C0 * R0C0 + matrix.R2C1 * R1C0 + matrix.R2C2 * R2C0;
|
||||||
|
result.R2C1 = matrix.R2C0 * R0C1 + matrix.R2C1 * R1C1 + matrix.R2C2 * R2C1;
|
||||||
|
result.R2C2 = matrix.R2C0 * R0C2 + matrix.R2C1 * R1C2 + matrix.R2C2 * R2C2;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>Multiply left matrix times left matrix.</summary>
|
||||||
|
/// <param name="matrix">The matrix on the matrix side of the equation.</param>
|
||||||
|
/// <param name="right">The matrix on the right side of the equation</param>
|
||||||
|
/// <param name="result">The resulting matrix of the multiplication.</param>
|
||||||
|
public static void Multiply(ref Matrix3d left, ref Matrix3d right, out Matrix3d result)
|
||||||
|
{
|
||||||
|
result.R0C0 = right.R0C0 * left.R0C0 + right.R0C1 * left.R1C0 + right.R0C2 * left.R2C0;
|
||||||
|
result.R0C1 = right.R0C0 * left.R0C1 + right.R0C1 * left.R1C1 + right.R0C2 * left.R2C1;
|
||||||
|
result.R0C2 = right.R0C0 * left.R0C2 + right.R0C1 * left.R1C2 + right.R0C2 * left.R2C2;
|
||||||
|
result.R1C0 = right.R1C0 * left.R0C0 + right.R1C1 * left.R1C0 + right.R1C2 * left.R2C0;
|
||||||
|
result.R1C1 = right.R1C0 * left.R0C1 + right.R1C1 * left.R1C1 + right.R1C2 * left.R2C1;
|
||||||
|
result.R1C2 = right.R1C0 * left.R0C2 + right.R1C1 * left.R1C2 + right.R1C2 * left.R2C2;
|
||||||
|
result.R2C0 = right.R2C0 * left.R0C0 + right.R2C1 * left.R1C0 + right.R2C2 * left.R2C0;
|
||||||
|
result.R2C1 = right.R2C0 * left.R0C1 + right.R2C1 * left.R1C1 + right.R2C2 * left.R2C1;
|
||||||
|
result.R2C2 = right.R2C0 * left.R0C2 + right.R2C1 * left.R1C2 + right.R2C2 * left.R2C2;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
/// <summary>Multiply matrix times this matrix.</summary>
|
||||||
|
/// <param name="matrix">The matrix to multiply.</param>
|
||||||
|
public void Multiply(double scalar)
|
||||||
|
{
|
||||||
|
R0C0 = scalar * R0C0;
|
||||||
|
R0C1 = scalar * R0C1;
|
||||||
|
R0C2 = scalar * R0C2;
|
||||||
|
R1C0 = scalar * R1C0;
|
||||||
|
R1C1 = scalar * R1C1;
|
||||||
|
R1C2 = scalar * R1C2;
|
||||||
|
R2C0 = scalar * R2C0;
|
||||||
|
R2C1 = scalar * R2C1;
|
||||||
|
R2C2 = scalar * R2C2;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>Multiply matrix times this matrix.</summary>
|
||||||
|
/// <param name="matrix">The matrix to multiply.</param>
|
||||||
|
/// <param name="result">The resulting matrix of the multiplication.</param>
|
||||||
|
public void Multiply(double scalar, out Matrix3d result)
|
||||||
|
{
|
||||||
|
result.R0C0 = scalar * R0C0;
|
||||||
|
result.R0C1 = scalar * R0C1;
|
||||||
|
result.R0C2 = scalar * R0C2;
|
||||||
|
result.R1C0 = scalar * R1C0;
|
||||||
|
result.R1C1 = scalar * R1C1;
|
||||||
|
result.R1C2 = scalar * R1C2;
|
||||||
|
result.R2C0 = scalar * R2C0;
|
||||||
|
result.R2C1 = scalar * R2C1;
|
||||||
|
result.R2C2 = scalar * R2C2;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>Multiply left matrix times left matrix.</summary>
|
||||||
|
/// <param name="matrix">The matrix on the matrix side of the equation.</param>
|
||||||
|
/// <param name="right">The matrix on the right side of the equation</param>
|
||||||
|
/// <param name="result">The resulting matrix of the multiplication.</param>
|
||||||
|
public static void Multiply(ref Matrix3d matrix, double scalar, out Matrix3d result)
|
||||||
|
{
|
||||||
|
result.R0C0 = scalar * matrix.R0C0;
|
||||||
|
result.R0C1 = scalar * matrix.R0C1;
|
||||||
|
result.R0C2 = scalar * matrix.R0C2;
|
||||||
|
result.R1C0 = scalar * matrix.R1C0;
|
||||||
|
result.R1C1 = scalar * matrix.R1C1;
|
||||||
|
result.R1C2 = scalar * matrix.R1C2;
|
||||||
|
result.R2C0 = scalar * matrix.R2C0;
|
||||||
|
result.R2C1 = scalar * matrix.R2C1;
|
||||||
|
result.R2C2 = scalar * matrix.R2C2;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
#endregion
|
||||||
|
|
||||||
|
|
||||||
|
#region Functions
|
||||||
|
|
||||||
|
public double Determinant
|
||||||
|
{
|
||||||
|
get
|
||||||
|
{
|
||||||
|
return R0C0 * R1C1 * R2C2 - R0C0 * R1C2 * R2C1 - R0C1 * R1C0 * R2C2 + R0C2 * R1C0 * R2C1 + R0C1 * R1C2 * R2C0 - R0C2 * R1C1 * R2C0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
public void Transpose()
|
||||||
|
{
|
||||||
|
Functions.Swap(ref R0C1, ref R1C0);
|
||||||
|
Functions.Swap(ref R0C2, ref R2C0);
|
||||||
|
Functions.Swap(ref R1C2, ref R2C1);
|
||||||
|
}
|
||||||
|
public void Transpose(out Matrix3d result)
|
||||||
|
{
|
||||||
|
result.R0C0 = R0C0;
|
||||||
|
result.R0C1 = R1C0;
|
||||||
|
result.R0C2 = R2C0;
|
||||||
|
result.R1C0 = R0C1;
|
||||||
|
result.R1C1 = R1C1;
|
||||||
|
result.R1C2 = R2C1;
|
||||||
|
result.R2C0 = R0C2;
|
||||||
|
result.R2C1 = R1C2;
|
||||||
|
result.R2C2 = R2C2;
|
||||||
|
}
|
||||||
|
public static void Transpose(ref Matrix3d matrix, out Matrix3d result)
|
||||||
|
{
|
||||||
|
result.R0C0 = matrix.R0C0;
|
||||||
|
result.R0C1 = matrix.R1C0;
|
||||||
|
result.R0C2 = matrix.R2C0;
|
||||||
|
result.R1C0 = matrix.R0C1;
|
||||||
|
result.R1C1 = matrix.R1C1;
|
||||||
|
result.R1C2 = matrix.R2C1;
|
||||||
|
result.R2C0 = matrix.R0C2;
|
||||||
|
result.R2C1 = matrix.R1C2;
|
||||||
|
result.R2C2 = matrix.R2C2;
|
||||||
|
}
|
||||||
|
|
||||||
|
#endregion
|
||||||
|
|
||||||
|
|
||||||
|
#region Transformation Functions
|
||||||
|
|
||||||
|
public void Transform(ref Vector3d vector)
|
||||||
|
{
|
||||||
|
double x = R0C0 * vector.X + R0C1 * vector.Y + R0C2 * vector.Z;
|
||||||
|
double y = R1C0 * vector.X + R1C1 * vector.Y + R1C2 * vector.Z;
|
||||||
|
vector.Z = R2C0 * vector.X + R2C1 * vector.Y + R2C2 * vector.Z;
|
||||||
|
vector.X = x;
|
||||||
|
vector.Y = y;
|
||||||
|
}
|
||||||
|
public static void Transform(ref Matrix3d matrix, ref Vector3d vector)
|
||||||
|
{
|
||||||
|
double x = matrix.R0C0 * vector.X + matrix.R0C1 * vector.Y + matrix.R0C2 * vector.Z;
|
||||||
|
double y = matrix.R1C0 * vector.X + matrix.R1C1 * vector.Y + matrix.R1C2 * vector.Z;
|
||||||
|
vector.Z = matrix.R2C0 * vector.X + matrix.R2C1 * vector.Y + matrix.R2C2 * vector.Z;
|
||||||
|
vector.X = x;
|
||||||
|
vector.Y = y;
|
||||||
|
}
|
||||||
|
public void Transform(ref Vector3d vector, out Vector3d result)
|
||||||
|
{
|
||||||
|
result.X = R0C0 * vector.X + R0C1 * vector.Y + R0C2 * vector.Z;
|
||||||
|
result.Y = R1C0 * vector.X + R1C1 * vector.Y + R1C2 * vector.Z;
|
||||||
|
result.Z = R2C0 * vector.X + R2C1 * vector.Y + R2C2 * vector.Z;
|
||||||
|
}
|
||||||
|
public static void Transform(ref Matrix3d matrix, ref Vector3d vector, out Vector3d result)
|
||||||
|
{
|
||||||
|
result.X = matrix.R0C0 * vector.X + matrix.R0C1 * vector.Y + matrix.R0C2 * vector.Z;
|
||||||
|
result.Y = matrix.R1C0 * vector.X + matrix.R1C1 * vector.Y + matrix.R1C2 * vector.Z;
|
||||||
|
result.Z = matrix.R2C0 * vector.X + matrix.R2C1 * vector.Y + matrix.R2C2 * vector.Z;
|
||||||
|
}
|
||||||
|
|
||||||
|
public void Rotate(double angle)
|
||||||
|
{
|
||||||
|
double angleRadians = Functions.DTOR * angle;
|
||||||
|
double sin = (double)System.Math.Sin(angleRadians);
|
||||||
|
double cos = (double)System.Math.Cos(angleRadians);
|
||||||
|
|
||||||
|
double r0c0 = cos * R0C0 + sin * R1C0;
|
||||||
|
double r0c1 = cos * R0C1 + sin * R1C1;
|
||||||
|
double r0c2 = cos * R0C2 + sin * R1C2;
|
||||||
|
|
||||||
|
R1C0 = cos * R1C0 - sin * R0C0;
|
||||||
|
R1C1 = cos * R1C1 - sin * R0C1;
|
||||||
|
R1C2 = cos * R1C2 - sin * R0C2;
|
||||||
|
|
||||||
|
R0C0 = r0c0;
|
||||||
|
R0C1 = r0c1;
|
||||||
|
R0C2 = r0c2;
|
||||||
|
}
|
||||||
|
public void Rotate(double angle, out Matrix3d result)
|
||||||
|
{
|
||||||
|
double angleRadians = Functions.DTOR * angle;
|
||||||
|
double sin = (double)System.Math.Sin(angleRadians);
|
||||||
|
double cos = (double)System.Math.Cos(angleRadians);
|
||||||
|
|
||||||
|
result.R0C0 = cos * R0C0 + sin * R1C0;
|
||||||
|
result.R0C1 = cos * R0C1 + sin * R1C1;
|
||||||
|
result.R0C2 = cos * R0C2 + sin * R1C2;
|
||||||
|
result.R1C0 = cos * R1C0 - sin * R0C0;
|
||||||
|
result.R1C1 = cos * R1C1 - sin * R0C1;
|
||||||
|
result.R1C2 = cos * R1C2 - sin * R0C2;
|
||||||
|
result.R2C0 = R2C0;
|
||||||
|
result.R2C1 = R2C1;
|
||||||
|
result.R2C2 = R2C2;
|
||||||
|
}
|
||||||
|
public static void Rotate(ref Matrix3d matrix, double angle, out Matrix3d result)
|
||||||
|
{
|
||||||
|
double angleRadians = Functions.DTOR * angle;
|
||||||
|
double sin = (double)System.Math.Sin(angleRadians);
|
||||||
|
double cos = (double)System.Math.Cos(angleRadians);
|
||||||
|
|
||||||
|
result.R0C0 = cos * matrix.R0C0 + sin * matrix.R1C0;
|
||||||
|
result.R0C1 = cos * matrix.R0C1 + sin * matrix.R1C1;
|
||||||
|
result.R0C2 = cos * matrix.R0C2 + sin * matrix.R1C2;
|
||||||
|
result.R1C0 = cos * matrix.R1C0 - sin * matrix.R0C0;
|
||||||
|
result.R1C1 = cos * matrix.R1C1 - sin * matrix.R0C1;
|
||||||
|
result.R1C2 = cos * matrix.R1C2 - sin * matrix.R0C2;
|
||||||
|
result.R2C0 = matrix.R2C0;
|
||||||
|
result.R2C1 = matrix.R2C1;
|
||||||
|
result.R2C2 = matrix.R2C2;
|
||||||
|
}
|
||||||
|
public static void RotateMatrix(double angle, out Matrix3d result)
|
||||||
|
{
|
||||||
|
double angleRadians = Functions.DTOR * angle;
|
||||||
|
double sin = (double)System.Math.Sin(angleRadians);
|
||||||
|
double cos = (double)System.Math.Cos(angleRadians);
|
||||||
|
|
||||||
|
result.R0C0 = cos;
|
||||||
|
result.R0C1 = sin;
|
||||||
|
result.R0C2 = 0;
|
||||||
|
result.R1C0 = -sin;
|
||||||
|
result.R1C1 = cos;
|
||||||
|
result.R1C2 = 0;
|
||||||
|
result.R2C0 = 0;
|
||||||
|
result.R2C1 = 0;
|
||||||
|
result.R2C2 = 1;
|
||||||
|
}
|
||||||
|
|
||||||
|
public void Quaternion(out Quaterniond quaternion)
|
||||||
|
{
|
||||||
|
quaternion = new Quaterniond(ref this);
|
||||||
|
}
|
||||||
|
|
||||||
|
#endregion
|
||||||
|
|
||||||
|
|
||||||
|
#region Constants
|
||||||
|
|
||||||
|
/// <summary>The identity matrix.</summary>
|
||||||
|
public static readonly Matrix3d Identity = new Matrix3d
|
||||||
|
(
|
||||||
|
1, 0, 0,
|
||||||
|
0, 1, 0,
|
||||||
|
0, 0, 1
|
||||||
|
);
|
||||||
|
|
||||||
|
/// <summary>A matrix of all zeros.</summary>
|
||||||
|
public static readonly Matrix3d Zero = new Matrix3d
|
||||||
|
(
|
||||||
|
0, 0, 0,
|
||||||
|
0, 0, 0,
|
||||||
|
0, 0, 0
|
||||||
|
);
|
||||||
|
|
||||||
|
#endregion
|
||||||
|
|
||||||
|
|
||||||
|
#region HashCode
|
||||||
|
|
||||||
|
/// <summary>Returns the hash code for this instance.</summary>
|
||||||
|
/// <returns>A 32-bit signed integer that is the hash code for this instance.</returns>
|
||||||
|
public override int GetHashCode()
|
||||||
|
{
|
||||||
|
return
|
||||||
|
R0C0.GetHashCode() ^ R0C1.GetHashCode() ^ R0C2.GetHashCode() ^
|
||||||
|
R1C0.GetHashCode() ^ R1C1.GetHashCode() ^ R1C2.GetHashCode() ^
|
||||||
|
R2C0.GetHashCode() ^ R2C1.GetHashCode() ^ R2C2.GetHashCode();
|
||||||
|
}
|
||||||
|
|
||||||
|
#endregion
|
||||||
|
|
||||||
|
|
||||||
|
#region String
|
||||||
|
|
||||||
|
/// <summary>Returns the fully qualified type name of this instance.</summary>
|
||||||
|
/// <returns>A System.String containing left fully qualified type name.</returns>
|
||||||
|
public override string ToString()
|
||||||
|
{
|
||||||
|
return String.Format(
|
||||||
|
"|{00}, {01}, {02}|\n" +
|
||||||
|
"|{03}, {04}, {05}|\n" +
|
||||||
|
"|{06}, {07}, {18}|\n" +
|
||||||
|
R0C0, R0C1, R0C2,
|
||||||
|
R1C0, R1C1, R1C2,
|
||||||
|
R2C0, R2C1, R2C2);
|
||||||
|
}
|
||||||
|
|
||||||
|
#endregion
|
||||||
|
}
|
||||||
|
}
|
||||||
2345
Source/OpenTK/Math/Matrix4d.cs
Normal file
2345
Source/OpenTK/Math/Matrix4d.cs
Normal file
File diff suppressed because it is too large
Load diff
638
Source/OpenTK/Math/Quaterniond.cs
Normal file
638
Source/OpenTK/Math/Quaterniond.cs
Normal file
|
|
@ -0,0 +1,638 @@
|
||||||
|
using System;
|
||||||
|
using System.Runtime.InteropServices;
|
||||||
|
using System.Text.RegularExpressions;
|
||||||
|
|
||||||
|
namespace OpenTK.Math
|
||||||
|
{
|
||||||
|
[Serializable]
|
||||||
|
[StructLayout(LayoutKind.Sequential)]
|
||||||
|
public struct Quaterniond
|
||||||
|
{
|
||||||
|
#region Fields
|
||||||
|
|
||||||
|
/// <summary>The W component of the quaternion.</summary>
|
||||||
|
public double W;
|
||||||
|
|
||||||
|
/// <summary>The X component of the quaternion.</summary>
|
||||||
|
public double X;
|
||||||
|
|
||||||
|
/// <summary>The Y component of the quaternion.</summary>
|
||||||
|
public double Y;
|
||||||
|
|
||||||
|
/// <summary>The Z component of the quaternion.</summary>
|
||||||
|
public double Z;
|
||||||
|
|
||||||
|
#endregion
|
||||||
|
|
||||||
|
|
||||||
|
#region Constructors
|
||||||
|
|
||||||
|
/// <summary>Constructs left quaternion that is left copy of the given quaternion.</summary>
|
||||||
|
/// <param name="quaterniond">The quaternion to copy.</param>
|
||||||
|
public Quaterniond(ref Quaterniond quaternion) : this(quaternion.W, quaternion.X, quaternion.Y, quaternion.Z) { }
|
||||||
|
|
||||||
|
/// <summary>Constructs left quaternion from the given components.</summary>
|
||||||
|
/// <param name="w">The W component for the quaternion.</param>
|
||||||
|
/// <param name="vector3d">A Vector representing the X, Y, and Z componets for the quaterion.</param>
|
||||||
|
public Quaterniond(double w, ref Vector3d vector3d) : this(w, vector3d.X, vector3d.Y, vector3d.Z) { }
|
||||||
|
|
||||||
|
/// <summary>Constructs left quaternion from the given axis and angle.</summary>
|
||||||
|
/// <param name="axis">The axis for the quaternion.</param>
|
||||||
|
/// <param name="angle">The angle for the quaternione.</param>
|
||||||
|
public Quaterniond(ref Vector3d axis, double angle)
|
||||||
|
{
|
||||||
|
double halfAngle = Functions.DTOR * angle / 2;
|
||||||
|
|
||||||
|
this.W = System.Math.Cos(halfAngle);
|
||||||
|
|
||||||
|
double sin = System.Math.Sin(halfAngle);
|
||||||
|
Vector3d axisNormalized;
|
||||||
|
axis.Normalize(out axisNormalized);
|
||||||
|
this.X = axisNormalized.X * sin;
|
||||||
|
this.Y = axisNormalized.Y * sin;
|
||||||
|
this.Z = axisNormalized.Z * sin;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>Constructs left quaternion from the given components.</summary>
|
||||||
|
/// <param name="w">The W component for the quaternion.</param>
|
||||||
|
/// <param name="x">The X component for the quaternion.</param>
|
||||||
|
/// <param name="y">The Y component for the quaternion.</param>
|
||||||
|
/// <param name="z">The Z component for the quaternion.</param>
|
||||||
|
public Quaterniond(double w, double x, double y, double z)
|
||||||
|
{
|
||||||
|
this.W = w;
|
||||||
|
this.X = x;
|
||||||
|
this.Y = y;
|
||||||
|
this.Z = z;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>Constructs left quaternion from the given array of double-precision floating point numbers.</summary>
|
||||||
|
/// <param name="doubleArray">The array of doubles for the components of the quaternion.</param>
|
||||||
|
public Quaterniond(double[] doubleArray)
|
||||||
|
{
|
||||||
|
if (doubleArray == null || doubleArray.GetLength(0) < 4) throw new MissingFieldException();
|
||||||
|
|
||||||
|
this.W = doubleArray[0];
|
||||||
|
this.X = doubleArray[1];
|
||||||
|
this.Y = doubleArray[2];
|
||||||
|
this.Z = doubleArray[3];
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>Constructs left quaternion from the given matrix. Only contains rotation information.</summary>
|
||||||
|
/// <param name="matrix">The matrix for the components of the quaternion.</param>
|
||||||
|
public Quaterniond(ref Matrix4d matrix)
|
||||||
|
{
|
||||||
|
double scale = System.Math.Pow(matrix.Determinant, 1.0d/3.0d);
|
||||||
|
|
||||||
|
W = System.Math.Sqrt(System.Math.Max(0, scale + matrix[0, 0] + matrix[1, 1] + matrix[2, 2])) / 2;
|
||||||
|
X = System.Math.Sqrt(System.Math.Max(0, scale + matrix[0, 0] - matrix[1, 1] - matrix[2, 2])) / 2;
|
||||||
|
Y = System.Math.Sqrt(System.Math.Max(0, scale - matrix[0, 0] + matrix[1, 1] - matrix[2, 2])) / 2;
|
||||||
|
Z = System.Math.Sqrt(System.Math.Max(0, scale - matrix[0, 0] - matrix[1, 1] + matrix[2, 2])) / 2;
|
||||||
|
if( matrix[2,1] - matrix[1,2] < 0 ) X = -X;
|
||||||
|
if( matrix[0,2] - matrix[2,0] < 0 ) Y = -Y;
|
||||||
|
if( matrix[1,0] - matrix[0,1] < 0 ) Z = -Z;
|
||||||
|
}
|
||||||
|
|
||||||
|
public Quaterniond(ref Matrix3d matrix)
|
||||||
|
{
|
||||||
|
double scale = System.Math.Pow(matrix.Determinant, 1.0d / 3.0d);
|
||||||
|
|
||||||
|
W = System.Math.Sqrt(System.Math.Max(0, scale + matrix[0, 0] + matrix[1, 1] + matrix[2, 2])) / 2;
|
||||||
|
X = System.Math.Sqrt(System.Math.Max(0, scale + matrix[0, 0] - matrix[1, 1] - matrix[2, 2])) / 2;
|
||||||
|
Y = System.Math.Sqrt(System.Math.Max(0, scale - matrix[0, 0] + matrix[1, 1] - matrix[2, 2])) / 2;
|
||||||
|
Z = System.Math.Sqrt(System.Math.Max(0, scale - matrix[0, 0] - matrix[1, 1] + matrix[2, 2])) / 2;
|
||||||
|
if (matrix[2, 1] - matrix[1, 2] < 0) X = -X;
|
||||||
|
if (matrix[0, 2] - matrix[2, 0] < 0) Y = -Y;
|
||||||
|
if (matrix[1, 0] - matrix[0, 1] < 0) Z = -Z;
|
||||||
|
}
|
||||||
|
|
||||||
|
#endregion
|
||||||
|
|
||||||
|
|
||||||
|
#region Arithmetic Operators
|
||||||
|
|
||||||
|
public void Add(ref Quaterniond quaternion)
|
||||||
|
{
|
||||||
|
W = W + quaternion.W;
|
||||||
|
X = X + quaternion.X;
|
||||||
|
Y = Y + quaternion.Y;
|
||||||
|
Z = Z + quaternion.Z;
|
||||||
|
}
|
||||||
|
public void Add(ref Quaterniond quaternion, out Quaterniond result)
|
||||||
|
{
|
||||||
|
result.W = W + quaternion.W;
|
||||||
|
result.X = X + quaternion.X;
|
||||||
|
result.Y = Y + quaternion.Y;
|
||||||
|
result.Z = Z + quaternion.Z;
|
||||||
|
}
|
||||||
|
public static void Add(ref Quaterniond left, ref Quaterniond right, out Quaterniond result)
|
||||||
|
{
|
||||||
|
result.W = left.W + right.W;
|
||||||
|
result.X = left.X + right.X;
|
||||||
|
result.Y = left.Y + right.Y;
|
||||||
|
result.Z = left.Z + right.Z;
|
||||||
|
}
|
||||||
|
|
||||||
|
public void Subtract(ref Quaterniond quaternion)
|
||||||
|
{
|
||||||
|
W = W - quaternion.W;
|
||||||
|
X = X - quaternion.X;
|
||||||
|
Y = Y - quaternion.Y;
|
||||||
|
Z = Z - quaternion.Z;
|
||||||
|
}
|
||||||
|
public void Subtract(ref Quaterniond quaternion, out Quaterniond result)
|
||||||
|
{
|
||||||
|
result.W = W - quaternion.W;
|
||||||
|
result.X = X - quaternion.X;
|
||||||
|
result.Y = Y - quaternion.Y;
|
||||||
|
result.Z = Z - quaternion.Z;
|
||||||
|
}
|
||||||
|
public static void Subtract(ref Quaterniond left, ref Quaterniond right, out Quaterniond result)
|
||||||
|
{
|
||||||
|
result.W = left.W - right.W;
|
||||||
|
result.X = left.X - right.X;
|
||||||
|
result.Y = left.Y - right.Y;
|
||||||
|
result.Z = left.Z - right.Z;
|
||||||
|
}
|
||||||
|
|
||||||
|
public void Multiply(ref Quaterniond quaternion)
|
||||||
|
{
|
||||||
|
double w = W * quaternion.W - X * quaternion.X - Y * quaternion.Y - Z * quaternion.Z;
|
||||||
|
double x = W * quaternion.X + X * quaternion.W + Y * quaternion.Z - Z * quaternion.Y;
|
||||||
|
double y = W * quaternion.Y + Y * quaternion.W + Z * quaternion.X - X * quaternion.Z;
|
||||||
|
Z = W * quaternion.Z + Z * quaternion.W + X * quaternion.Y - Y * quaternion.X;
|
||||||
|
W = w;
|
||||||
|
X = x;
|
||||||
|
Y = y;
|
||||||
|
}
|
||||||
|
public void Multiply(ref Quaterniond quaternion, out Quaterniond result)
|
||||||
|
{
|
||||||
|
result.W = W * quaternion.W - X * quaternion.X - Y * quaternion.Y - Z * quaternion.Z;
|
||||||
|
result.X = W * quaternion.X + X * quaternion.W + Y * quaternion.Z - Z * quaternion.Y;
|
||||||
|
result.Y = W * quaternion.Y + Y * quaternion.W + Z * quaternion.X - X * quaternion.Z;
|
||||||
|
result.Z = W * quaternion.Z + Z * quaternion.W + X * quaternion.Y - Y * quaternion.X;
|
||||||
|
}
|
||||||
|
public static void Multiply(ref Quaterniond left, ref Quaterniond right, out Quaterniond result)
|
||||||
|
{
|
||||||
|
result.W = left.W * right.W - left.X * right.X - left.Y * right.Y - left.Z * right.Z;
|
||||||
|
result.X = left.W * right.X + left.X * right.W + left.Y * right.Z - left.Z * right.Y;
|
||||||
|
result.Y = left.W * right.Y + left.Y * right.W + left.Z * right.X - left.X * right.Z;
|
||||||
|
result.Z = left.W * right.Z + left.Z * right.W + left.X * right.Y - left.Y * right.X;
|
||||||
|
}
|
||||||
|
|
||||||
|
public void Multiply(double scalar)
|
||||||
|
{
|
||||||
|
W = W * scalar;
|
||||||
|
X = X * scalar;
|
||||||
|
Y = Y * scalar;
|
||||||
|
Z = Z * scalar;
|
||||||
|
}
|
||||||
|
public void Multiply(double scalar, out Quaterniond result)
|
||||||
|
{
|
||||||
|
result.W = W * scalar;
|
||||||
|
result.X = X * scalar;
|
||||||
|
result.Y = Y * scalar;
|
||||||
|
result.Z = Z * scalar;
|
||||||
|
}
|
||||||
|
public static void Multiply(ref Quaterniond quaternion, double scalar, out Quaterniond result)
|
||||||
|
{
|
||||||
|
result.W = quaternion.W * scalar;
|
||||||
|
result.X = quaternion.X * scalar;
|
||||||
|
result.Y = quaternion.Y * scalar;
|
||||||
|
result.Z = quaternion.Z * scalar;
|
||||||
|
}
|
||||||
|
|
||||||
|
public void Divide(double scalar)
|
||||||
|
{
|
||||||
|
if (scalar == 0) throw new DivideByZeroException();
|
||||||
|
W = W / scalar;
|
||||||
|
X = X / scalar;
|
||||||
|
Y = Y / scalar;
|
||||||
|
Z = Z / scalar;
|
||||||
|
}
|
||||||
|
public void Divide(double scalar, out Quaterniond result)
|
||||||
|
{
|
||||||
|
if (scalar == 0) throw new DivideByZeroException();
|
||||||
|
result.W = W / scalar;
|
||||||
|
result.X = X / scalar;
|
||||||
|
result.Y = Y / scalar;
|
||||||
|
result.Z = Z / scalar;
|
||||||
|
}
|
||||||
|
public static void Divide(ref Quaterniond quaternion, double scalar, out Quaterniond result)
|
||||||
|
{
|
||||||
|
if (scalar == 0) throw new DivideByZeroException();
|
||||||
|
result.W = quaternion.W / scalar;
|
||||||
|
result.X = quaternion.X / scalar;
|
||||||
|
result.Y = quaternion.Y / scalar;
|
||||||
|
result.Z = quaternion.Z / scalar;
|
||||||
|
}
|
||||||
|
|
||||||
|
#endregion
|
||||||
|
|
||||||
|
|
||||||
|
#region Functions
|
||||||
|
|
||||||
|
public double Modulus
|
||||||
|
{
|
||||||
|
get
|
||||||
|
{
|
||||||
|
return System.Math.Sqrt(W * W + X * X + Y * Y + Z * Z);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
public double ModulusSquared
|
||||||
|
{
|
||||||
|
get
|
||||||
|
{
|
||||||
|
return W * W + X * X + Y * Y + Z * Z;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
public static double DotProduct(Quaterniond left, Quaterniond right)
|
||||||
|
{
|
||||||
|
return left.W * right.W + left.X * right.X + left.Y * right.Y + left.Z * right.Z;
|
||||||
|
}
|
||||||
|
|
||||||
|
public void Normalize()
|
||||||
|
{
|
||||||
|
double modulus = System.Math.Sqrt(W * W + X * X + Y * Y + Z * Z);
|
||||||
|
if (modulus == 0) throw new DivideByZeroException();
|
||||||
|
W = W / modulus;
|
||||||
|
X = X / modulus;
|
||||||
|
Y = Y / modulus;
|
||||||
|
Z = Z / modulus;
|
||||||
|
}
|
||||||
|
public void Normalize( out Quaterniond result )
|
||||||
|
{
|
||||||
|
double modulus = System.Math.Sqrt(W * W + X * X + Y * Y + Z * Z);
|
||||||
|
if (modulus == 0) throw new DivideByZeroException();
|
||||||
|
result.W = W / modulus;
|
||||||
|
result.X = X / modulus;
|
||||||
|
result.Y = Y / modulus;
|
||||||
|
result.Z = Z / modulus;
|
||||||
|
}
|
||||||
|
public static void Normalize(ref Quaterniond quaternion, out Quaterniond result)
|
||||||
|
{
|
||||||
|
double modulus = System.Math.Sqrt(quaternion.W * quaternion.W + quaternion.X * quaternion.X + quaternion.Y * quaternion.Y + quaternion.Z * quaternion.Z);
|
||||||
|
if (modulus == 0) throw new DivideByZeroException();
|
||||||
|
result.W = quaternion.W / modulus;
|
||||||
|
result.X = quaternion.X / modulus;
|
||||||
|
result.Y = quaternion.Y / modulus;
|
||||||
|
result.Z = quaternion.Z / modulus;
|
||||||
|
}
|
||||||
|
|
||||||
|
public void Conjugate()
|
||||||
|
{
|
||||||
|
X = -X;
|
||||||
|
Y = -Y;
|
||||||
|
Z = -Z;
|
||||||
|
}
|
||||||
|
public void Conjugate( out Quaterniond result )
|
||||||
|
{
|
||||||
|
result.W = W;
|
||||||
|
result.X = -X;
|
||||||
|
result.Y = -Y;
|
||||||
|
result.Z = -Z;
|
||||||
|
}
|
||||||
|
public static void Conjugate(ref Quaterniond quaternion, out Quaterniond result)
|
||||||
|
{
|
||||||
|
result.W = quaternion.W;
|
||||||
|
result.X = -quaternion.X;
|
||||||
|
result.Y = -quaternion.Y;
|
||||||
|
result.Z = -quaternion.Z;
|
||||||
|
}
|
||||||
|
|
||||||
|
public void Inverse()
|
||||||
|
{
|
||||||
|
double modulusSquared = W * W + X * X + Y * Y + Z * Z;
|
||||||
|
if (modulusSquared <= 0) throw new InvalidOperationException();
|
||||||
|
double inverseModulusSquared = 1.0 / modulusSquared;
|
||||||
|
W = W * inverseModulusSquared;
|
||||||
|
X = X * -inverseModulusSquared;
|
||||||
|
Y = Y * -inverseModulusSquared;
|
||||||
|
Z = Z * -inverseModulusSquared;
|
||||||
|
}
|
||||||
|
public void Inverse( out Quaterniond result )
|
||||||
|
{
|
||||||
|
double modulusSquared = W * W + X * X + Y * Y + Z * Z;
|
||||||
|
if (modulusSquared <= 0) throw new InvalidOperationException();
|
||||||
|
double inverseModulusSquared = 1.0 / modulusSquared;
|
||||||
|
result.W = W * inverseModulusSquared;
|
||||||
|
result.X = X * -inverseModulusSquared;
|
||||||
|
result.Y = Y * -inverseModulusSquared;
|
||||||
|
result.Z = Z * -inverseModulusSquared;
|
||||||
|
}
|
||||||
|
public static void Inverse(ref Quaterniond quaternion, out Quaterniond result)
|
||||||
|
{
|
||||||
|
double modulusSquared = quaternion.W * quaternion.W + quaternion.X * quaternion.X + quaternion.Y * quaternion.Y + quaternion.Z * quaternion.Z;
|
||||||
|
if (modulusSquared <= 0) throw new InvalidOperationException();
|
||||||
|
double inverseModulusSquared = 1.0 / modulusSquared;
|
||||||
|
result.W = quaternion.W * inverseModulusSquared;
|
||||||
|
result.X = quaternion.X * -inverseModulusSquared;
|
||||||
|
result.Y = quaternion.Y * -inverseModulusSquared;
|
||||||
|
result.Z = quaternion.Z * -inverseModulusSquared;
|
||||||
|
}
|
||||||
|
|
||||||
|
public void Log()
|
||||||
|
{
|
||||||
|
if (System.Math.Abs(W) < 1.0)
|
||||||
|
{
|
||||||
|
double angle = System.Math.Acos(W);
|
||||||
|
double sin = System.Math.Sin(angle);
|
||||||
|
|
||||||
|
if (System.Math.Abs(sin) >= 0)
|
||||||
|
{
|
||||||
|
double coefficient = angle / sin;
|
||||||
|
X = X * coefficient;
|
||||||
|
Y = Y * coefficient;
|
||||||
|
Z = Z * coefficient;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
X = 0;
|
||||||
|
Y = 0;
|
||||||
|
Z = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
W = 0;
|
||||||
|
}
|
||||||
|
public void Log( out Quaterniond result )
|
||||||
|
{
|
||||||
|
if (System.Math.Abs(W) < 1.0)
|
||||||
|
{
|
||||||
|
double angle = System.Math.Acos(W);
|
||||||
|
double sin = System.Math.Sin(angle);
|
||||||
|
|
||||||
|
if (System.Math.Abs(sin) >= 0)
|
||||||
|
{
|
||||||
|
double coefficient = angle / sin;
|
||||||
|
result.X = X * coefficient;
|
||||||
|
result.Y = Y * coefficient;
|
||||||
|
result.Z = Z * coefficient;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
result.X = X;
|
||||||
|
result.Y = Y;
|
||||||
|
result.Z = Z;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
result.X = 0;
|
||||||
|
result.Y = 0;
|
||||||
|
result.Z = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
result.W = 0;
|
||||||
|
}
|
||||||
|
public static void Log(ref Quaterniond quaternion, out Quaterniond result)
|
||||||
|
{
|
||||||
|
if (System.Math.Abs(quaternion.W) < 1.0)
|
||||||
|
{
|
||||||
|
double angle = System.Math.Acos(quaternion.W);
|
||||||
|
double sin = System.Math.Sin(angle);
|
||||||
|
|
||||||
|
if (System.Math.Abs(sin) >= 0)
|
||||||
|
{
|
||||||
|
double coefficient = angle / sin;
|
||||||
|
result.X = quaternion.X * coefficient;
|
||||||
|
result.Y = quaternion.Y * coefficient;
|
||||||
|
result.Z = quaternion.Z * coefficient;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
result.X = quaternion.X;
|
||||||
|
result.Y = quaternion.Y;
|
||||||
|
result.Z = quaternion.Z;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
result.X = 0;
|
||||||
|
result.Y = 0;
|
||||||
|
result.Z = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
result.W = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
public void Exp()
|
||||||
|
{
|
||||||
|
double angle = System.Math.Sqrt(X * X + Y * Y + Z * Z);
|
||||||
|
double sin = System.Math.Sin(angle);
|
||||||
|
|
||||||
|
if (System.Math.Abs(sin) > 0)
|
||||||
|
{
|
||||||
|
double coefficient = angle / sin;
|
||||||
|
W = 0;
|
||||||
|
X = X * coefficient;
|
||||||
|
Y = Y * coefficient;
|
||||||
|
Z = Z * coefficient;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
W = 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
public void Exp(out Quaterniond result)
|
||||||
|
{
|
||||||
|
double angle = System.Math.Sqrt(X * X + Y * Y + Z * Z);
|
||||||
|
double sin = System.Math.Sin(angle);
|
||||||
|
|
||||||
|
if (System.Math.Abs(sin) > 0)
|
||||||
|
{
|
||||||
|
double coefficient = angle / sin;
|
||||||
|
result.W = 0;
|
||||||
|
result.X = X * coefficient;
|
||||||
|
result.Y = Y * coefficient;
|
||||||
|
result.Z = Z * coefficient;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
result.W = 0;
|
||||||
|
result.X = X;
|
||||||
|
result.Y = Y;
|
||||||
|
result.Z = Z;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
public static void Exp(ref Quaterniond quaternion, out Quaterniond result)
|
||||||
|
{
|
||||||
|
double angle = System.Math.Sqrt(quaternion.X * quaternion.X + quaternion.Y * quaternion.Y + quaternion.Z * quaternion.Z);
|
||||||
|
double sin = System.Math.Sin(angle);
|
||||||
|
|
||||||
|
if (System.Math.Abs(sin) > 0)
|
||||||
|
{
|
||||||
|
double coefficient = angle / sin;
|
||||||
|
result.W = 0;
|
||||||
|
result.X = quaternion.X * coefficient;
|
||||||
|
result.Y = quaternion.Y * coefficient;
|
||||||
|
result.Z = quaternion.Z * coefficient;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
result.W = 0;
|
||||||
|
result.X = quaternion.X;
|
||||||
|
result.Y = quaternion.Y;
|
||||||
|
result.Z = quaternion.Z;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>Returns left matrix for this quaternion.</summary>
|
||||||
|
public void Matrix4(out Matrix4d result)
|
||||||
|
{
|
||||||
|
// TODO Expand
|
||||||
|
result = new Matrix4d(ref this);
|
||||||
|
}
|
||||||
|
|
||||||
|
public void GetAxisAndAngle(out Vector3d axis, out double angle)
|
||||||
|
{
|
||||||
|
Quaterniond quaternion;
|
||||||
|
Normalize(out quaternion);
|
||||||
|
double cos = quaternion.W;
|
||||||
|
angle = System.Math.Acos(cos) * 2 * Functions.RTOD;
|
||||||
|
double sin = System.Math.Sqrt( 1.0d - cos * cos );
|
||||||
|
if ( System.Math.Abs( sin ) < 0.0001 ) sin = 1;
|
||||||
|
axis = new Vector3d(X / sin, Y / sin, Z / sin);
|
||||||
|
}
|
||||||
|
|
||||||
|
public static void Slerp(ref Quaterniond start, ref Quaterniond end, double blend, out Quaterniond result)
|
||||||
|
{
|
||||||
|
if (start.W == 0 && start.X == 0 && start.Y == 0 && start.Z == 0)
|
||||||
|
{
|
||||||
|
if (end.W == 0 && end.X == 0 && end.Y == 0 && end.Z == 0)
|
||||||
|
{
|
||||||
|
result.W = 1;
|
||||||
|
result.X = 0;
|
||||||
|
result.Y = 0;
|
||||||
|
result.Z = 0;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
result = end;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else if (end.W == 0 && end.X == 0 && end.Y == 0 && end.Z == 0)
|
||||||
|
{
|
||||||
|
result = start;
|
||||||
|
}
|
||||||
|
|
||||||
|
Vector3d startVector = new Vector3d(start.X, start.Y, start.Z);
|
||||||
|
Vector3d endVector = new Vector3d(end.X, end.Y, end.Z);
|
||||||
|
double cosHalfAngle = start.W * end.W + Vector3d.DotProduct(ref startVector, ref endVector);
|
||||||
|
|
||||||
|
if (cosHalfAngle >= 1.0f || cosHalfAngle <= -1.0f)
|
||||||
|
{
|
||||||
|
// angle = 0.0f, so just return one input.
|
||||||
|
result = start;
|
||||||
|
}
|
||||||
|
else if (cosHalfAngle < 0.0f)
|
||||||
|
{
|
||||||
|
end.W = -end.W;
|
||||||
|
end.X = -end.X;
|
||||||
|
end.Y = -end.Y;
|
||||||
|
end.Z = -end.Z;
|
||||||
|
cosHalfAngle = -cosHalfAngle;
|
||||||
|
}
|
||||||
|
|
||||||
|
double blendA;
|
||||||
|
double blendB;
|
||||||
|
if (cosHalfAngle < 0.99f)
|
||||||
|
{
|
||||||
|
// do proper slerp for big angles
|
||||||
|
double halfAngle = (double)System.Math.Acos(cosHalfAngle);
|
||||||
|
double sinHalfAngle = (double)System.Math.Sin(halfAngle);
|
||||||
|
double oneOverSinHalfAngle = 1.0f / sinHalfAngle;
|
||||||
|
blendA = (double)System.Math.Sin(halfAngle * (1.0f - blend)) * oneOverSinHalfAngle;
|
||||||
|
blendB = (double)System.Math.Sin(halfAngle * blend) * oneOverSinHalfAngle;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
// do lerp if angle is really small.
|
||||||
|
blendA = 1.0f - blend;
|
||||||
|
blendB = blend;
|
||||||
|
}
|
||||||
|
|
||||||
|
result.W = blendA * start.W + blendB * end.W;
|
||||||
|
result.X = blendA * start.X + blendB * end.X;
|
||||||
|
result.Y = blendA * start.Y + blendB * end.Y;
|
||||||
|
result.Z = blendA * start.Z + blendB * end.Z;
|
||||||
|
|
||||||
|
if (result.W != 0 || result.X != 0 || result.Y != 0 || result.Z != 0)
|
||||||
|
{
|
||||||
|
result.Normalize();
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
result.W = 1;
|
||||||
|
result.X = 0;
|
||||||
|
result.Y = 0;
|
||||||
|
result.Z = 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
#endregion
|
||||||
|
|
||||||
|
|
||||||
|
#region HashCode
|
||||||
|
|
||||||
|
/// <summary>Returns the hash code for this instance.</summary>
|
||||||
|
/// <returns>A 32-bit signed integer that is the hash code for this instance.</returns>
|
||||||
|
public override int GetHashCode()
|
||||||
|
{
|
||||||
|
base.GetHashCode();
|
||||||
|
return W.GetHashCode() ^ X.GetHashCode() ^ Y.GetHashCode() ^ Z.GetHashCode();
|
||||||
|
}
|
||||||
|
|
||||||
|
#endregion
|
||||||
|
|
||||||
|
|
||||||
|
#region String and Parse
|
||||||
|
|
||||||
|
/// <summary>Returns the fully qualified type name of this instance.</summary>
|
||||||
|
/// <returns>A System.String containing left fully qualified type name.</returns>
|
||||||
|
public override string ToString()
|
||||||
|
{
|
||||||
|
return string.Format("({0}, {1}, {2}, {3})", W, X, Y, Z);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>Parses left string, converting it to left quaternion.</summary>
|
||||||
|
/// <param name="str">The string to parse.</param>
|
||||||
|
/// <returns>The quaternion represented by the string.</returns>
|
||||||
|
public static void Parse(string str, out Quaterniond result)
|
||||||
|
{
|
||||||
|
Match match = new Regex(@"\((?<w>.*),(?<x>.*),(?<y>.*),(?<z>.*)\)", RegexOptions.None).Match(str);
|
||||||
|
if (!match.Success) throw new Exception("Parse failed!");
|
||||||
|
|
||||||
|
result.W = double.Parse(match.Result("${w}"));
|
||||||
|
result.X = double.Parse(match.Result("${x}"));
|
||||||
|
result.Y = double.Parse(match.Result("${y}"));
|
||||||
|
result.Z = double.Parse(match.Result("${z}"));
|
||||||
|
}
|
||||||
|
|
||||||
|
#endregion
|
||||||
|
|
||||||
|
|
||||||
|
#region Constants
|
||||||
|
|
||||||
|
/// <summary>A quaterion with all zero components.</summary>
|
||||||
|
public static readonly Quaterniond Zero = new Quaterniond(0, 0, 0, 0);
|
||||||
|
|
||||||
|
/// <summary>A quaterion representing an identity.</summary>
|
||||||
|
public static readonly Quaterniond Identity = new Quaterniond(1, 0, 0, 0);
|
||||||
|
|
||||||
|
/// <summary>A quaterion representing the W axis.</summary>
|
||||||
|
public static readonly Quaterniond WAxis = new Quaterniond(1, 0, 0, 0);
|
||||||
|
|
||||||
|
/// <summary>A quaterion representing the X axis.</summary>
|
||||||
|
public static readonly Quaterniond XAxis = new Quaterniond(0, 1, 0, 0);
|
||||||
|
|
||||||
|
/// <summary>A quaterion representing the Y axis.</summary>
|
||||||
|
public static readonly Quaterniond YAxis = new Quaterniond(0, 0, 1, 0);
|
||||||
|
|
||||||
|
/// <summary>A quaterion representing the Z axis.</summary>
|
||||||
|
public static readonly Quaterniond ZAxis = new Quaterniond(0, 0, 0, 1);
|
||||||
|
|
||||||
|
#endregion
|
||||||
|
}
|
||||||
|
}
|
||||||
921
Source/OpenTK/Math/Vector2d.cs
Normal file
921
Source/OpenTK/Math/Vector2d.cs
Normal file
|
|
@ -0,0 +1,921 @@
|
||||||
|
using System;
|
||||||
|
using System.Diagnostics;
|
||||||
|
using System.Collections.Generic;
|
||||||
|
using System.Runtime.InteropServices;
|
||||||
|
using System.Text.RegularExpressions;
|
||||||
|
|
||||||
|
namespace OpenTK.Math
|
||||||
|
{
|
||||||
|
/// <summary>A 2-dimensional vector using double-precision floating point numbers.</summary>
|
||||||
|
[Serializable]
|
||||||
|
[StructLayout(LayoutKind.Sequential)]
|
||||||
|
public struct Vector2d : IEquatable<Vector2d>, IComparer<Vector2d>, IComparable<Vector2d>
|
||||||
|
{
|
||||||
|
#region Fields & Access
|
||||||
|
|
||||||
|
/// <summary>The X coordinate of the vector.</summary>
|
||||||
|
public double X;
|
||||||
|
|
||||||
|
/// <summary>The Y coordinate of the vector.</summary>
|
||||||
|
public double Y;
|
||||||
|
|
||||||
|
/// <summary>The coordinate at the index of the vector.</summary>
|
||||||
|
public double this[int index]
|
||||||
|
{
|
||||||
|
get
|
||||||
|
{
|
||||||
|
switch (index)
|
||||||
|
{
|
||||||
|
case 0:
|
||||||
|
return X;
|
||||||
|
|
||||||
|
case 1:
|
||||||
|
return Y;
|
||||||
|
}
|
||||||
|
|
||||||
|
throw new IndexOutOfRangeException();
|
||||||
|
}
|
||||||
|
set
|
||||||
|
{
|
||||||
|
switch (index)
|
||||||
|
{
|
||||||
|
case 0:
|
||||||
|
X = value;
|
||||||
|
return;
|
||||||
|
|
||||||
|
case 1:
|
||||||
|
Y = value;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
throw new IndexOutOfRangeException();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>Converts the vector into an array of double-precision floating point numbers.</summary>
|
||||||
|
/// <param name="vector">The vector being converted.</param>
|
||||||
|
/// <returns>An array of double-precision floating point numbers representing the vector coordinates.</returns>
|
||||||
|
public static explicit operator double[](Vector2d vector)
|
||||||
|
{
|
||||||
|
return new double[2] { vector.X, vector.Y };
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>Converts the vector into left double-precision floating point number pointer.</summary>
|
||||||
|
/// <param name="vector">The vector being converted.</param>
|
||||||
|
/// <returns>A double-precision floating point number pointer to the vector coordinates.</returns>
|
||||||
|
unsafe public static explicit operator double*(Vector2d vector)
|
||||||
|
{
|
||||||
|
return &vector.X;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>Converts the vector into an IntPtr.</summary>
|
||||||
|
/// <param name="vector">The vector being converted.</param>
|
||||||
|
/// <returns>An IntPtr to the vector coordinates.</returns>
|
||||||
|
public static explicit operator IntPtr(Vector2d vector)
|
||||||
|
{
|
||||||
|
unsafe
|
||||||
|
{
|
||||||
|
return (IntPtr)(&vector.X);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
#endregion
|
||||||
|
|
||||||
|
|
||||||
|
#region Constructors
|
||||||
|
|
||||||
|
/// <summary>Constructs left vector with the given coordinates.</summary>
|
||||||
|
/// <param name="x">The X coordinate.</param>
|
||||||
|
/// <param name="y">The Y coordinate.</param>
|
||||||
|
public Vector2d(double x, double y)
|
||||||
|
{
|
||||||
|
this.X = x;
|
||||||
|
this.Y = y;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>Constructs left vector with the same coordinates as the given vector.</summary>
|
||||||
|
/// <param name="vector">The vector whose coordinates to copy.</param>
|
||||||
|
public Vector2d(ref Vector2d vector)
|
||||||
|
{
|
||||||
|
this.X = vector.X;
|
||||||
|
this.Y = vector.Y;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>Constructs left vector from the given array of double-precision floating point numbers.</summary>
|
||||||
|
/// <param name="doubleArray">The array of doubles for the coordinates of the vector.</param>
|
||||||
|
public Vector2d(double[] coordinateArray)
|
||||||
|
{
|
||||||
|
if (coordinateArray == null || coordinateArray.GetLength(0) < 2) throw new Exception("Invalid parameter.");
|
||||||
|
|
||||||
|
this.X = coordinateArray[0];
|
||||||
|
this.Y = coordinateArray[1];
|
||||||
|
}
|
||||||
|
|
||||||
|
#endregion
|
||||||
|
|
||||||
|
|
||||||
|
#region Equality
|
||||||
|
|
||||||
|
/// <summary>Indicates whether the current vector is equal to another vector.</summary>
|
||||||
|
/// <param name="vector">An vector to compare with this vector.</param>
|
||||||
|
/// <returns>true if the current vector is equal to the vector parameter; otherwise, false.</returns>
|
||||||
|
public bool Equals(Vector2d vector)
|
||||||
|
{
|
||||||
|
return
|
||||||
|
X == vector.X &&
|
||||||
|
Y == vector.Y;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>Indicates whether the current vector is equal to another vector.</summary>
|
||||||
|
/// <param name="vector">An vector to compare with this vector.</param>
|
||||||
|
/// <returns>true if the current vector is equal to the vector parameter; otherwise, false.</returns>
|
||||||
|
public bool Equals(ref Vector2d vector)
|
||||||
|
{
|
||||||
|
return
|
||||||
|
X == vector.X &&
|
||||||
|
Y == vector.Y;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>Indicates whether two vectors are approximately equal to each other.</summary>
|
||||||
|
/// <param name="matrix">The first vector.</param>
|
||||||
|
/// <param name="right">The second vector.</param>
|
||||||
|
/// <returns>true if the vectors are approximately equal; otherwise, false.</returns>
|
||||||
|
public static bool Equals(ref Vector2d left, ref Vector2d right)
|
||||||
|
{
|
||||||
|
return
|
||||||
|
left.X == right.X &&
|
||||||
|
left.Y == right.Y;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>Indicates whether the current vector is equal to another vector.</summary>
|
||||||
|
/// <param name="vector">An vector to compare with this vector.</param>
|
||||||
|
/// <returns>true if the current vector is equal to the vector parameter; otherwise, false.</returns>
|
||||||
|
public bool EqualsApprox(ref Vector2d vector, double tolerance)
|
||||||
|
{
|
||||||
|
return
|
||||||
|
System.Math.Abs(X - vector.X) <= tolerance &&
|
||||||
|
System.Math.Abs(Y - vector.Y) <= tolerance;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>Indicates whether two vectors are approximately equal to each other within left given tolerance.</summary>
|
||||||
|
/// <param name="matrix">The first vector.</param>
|
||||||
|
/// <param name="right">The second vector.</param>
|
||||||
|
/// <param name="tolerance">The tolerance for the approximation.</param>
|
||||||
|
/// <returns>true if the vectors are approximately equal; otherwise, false.</returns>
|
||||||
|
public static bool EqualsApprox(ref Vector2d left, ref Vector2d right, double tolerance)
|
||||||
|
{
|
||||||
|
return
|
||||||
|
System.Math.Abs(left.X - right.X) <= tolerance &&
|
||||||
|
System.Math.Abs(left.Y - right.Y) <= tolerance;
|
||||||
|
}
|
||||||
|
|
||||||
|
#endregion
|
||||||
|
|
||||||
|
|
||||||
|
#region IComparer
|
||||||
|
|
||||||
|
/// <summary>Compares two vectors and returns left value indicating whether one is less than, equal to, or greater than the other.</summary>
|
||||||
|
public int Compare(Vector2d left, Vector2d right)
|
||||||
|
{
|
||||||
|
if (left.X != right.X)
|
||||||
|
{
|
||||||
|
if (left.X < right.X) return -1;
|
||||||
|
else return 1;
|
||||||
|
}
|
||||||
|
else if (left.Y != right.Y)
|
||||||
|
{
|
||||||
|
if (left.Y < right.Y) return -1;
|
||||||
|
else return 1;
|
||||||
|
}
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
#endregion
|
||||||
|
|
||||||
|
|
||||||
|
#region IComparable
|
||||||
|
|
||||||
|
/// <summary>Compares the vector with another vector and returns left value indicating whether it is less than, equal to, or greater than the other vector.</summary>
|
||||||
|
public int CompareTo(Vector2d vector) { return Compare(this, vector); }
|
||||||
|
|
||||||
|
#endregion
|
||||||
|
|
||||||
|
|
||||||
|
#region Length
|
||||||
|
|
||||||
|
/// <summary>Gets the length of the vector.</summary>
|
||||||
|
public double Length
|
||||||
|
{
|
||||||
|
get
|
||||||
|
{
|
||||||
|
double lengthSquared = LengthSquared;
|
||||||
|
|
||||||
|
if (lengthSquared == 1)
|
||||||
|
{
|
||||||
|
return 1;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
return System.Math.Sqrt(lengthSquared);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>Gets the squared length of the vector.</summary>
|
||||||
|
public double LengthSquared
|
||||||
|
{
|
||||||
|
get
|
||||||
|
{
|
||||||
|
return X * X + Y * Y;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>Gets the approimate length of the vector.</summary>
|
||||||
|
public double LengthApprox
|
||||||
|
{
|
||||||
|
get
|
||||||
|
{
|
||||||
|
return 1.0d / Functions.InverseSqrtFast(X * X + Y * Y);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
#endregion
|
||||||
|
|
||||||
|
|
||||||
|
#region Distance
|
||||||
|
|
||||||
|
/// <summary>Gets the distance from this vector to the given vector.</summary>
|
||||||
|
/// <param name="vector">The vector to which to find the distance.</param>
|
||||||
|
/// <returns>The distance from this vector to the given vector.</returns>
|
||||||
|
public double DistanceTo(ref Vector2d vector)
|
||||||
|
{
|
||||||
|
double deltaX = vector.X - X;
|
||||||
|
double deltaY = vector.Y - Y;
|
||||||
|
return System.Math.Sqrt(deltaX * deltaX + deltaY * deltaY);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>Gets the squared distance from this vector to the given vector.</summary>
|
||||||
|
/// <param name="vector">The vector to which to find the squared distance.</param>
|
||||||
|
/// <returns>The squared distance from this vector to the given vector.</returns>
|
||||||
|
public double DistanceSquaredTo(ref Vector2d vector)
|
||||||
|
{
|
||||||
|
double deltaX = vector.X - X;
|
||||||
|
double deltaY = vector.Y - Y;
|
||||||
|
return deltaX * deltaX + deltaY * deltaY;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>Gets the approximate distance from this vector to the given vector.</summary>
|
||||||
|
/// <param name="vector">The vector to which to find the approximate distance.</param>
|
||||||
|
/// <returns>The approximate distance from this vector to the given vector.</returns>
|
||||||
|
public double DistanceApproxTo(ref Vector2d vector)
|
||||||
|
{
|
||||||
|
double deltaX = vector.X - X;
|
||||||
|
double deltaY = vector.Y - Y;
|
||||||
|
return 1.0d / Functions.InverseSqrtFast(deltaX * deltaX + deltaY * deltaY);
|
||||||
|
}
|
||||||
|
|
||||||
|
#endregion
|
||||||
|
|
||||||
|
|
||||||
|
#region Normalize
|
||||||
|
|
||||||
|
/// <summary>Normalize this vector.</summary>
|
||||||
|
public void Normalize()
|
||||||
|
{
|
||||||
|
double lengthSquared = LengthSquared;
|
||||||
|
|
||||||
|
if (lengthSquared != 1 && lengthSquared != 0)
|
||||||
|
{
|
||||||
|
double length = System.Math.Sqrt(lengthSquared);
|
||||||
|
X = X / length;
|
||||||
|
Y = Y / length;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>Get the normalized version of this vector.</summary>
|
||||||
|
/// <param name="result">The resulting normalized vector.</param>
|
||||||
|
public void Normalize(out Vector2d result)
|
||||||
|
{
|
||||||
|
double lengthSquared = LengthSquared;
|
||||||
|
|
||||||
|
if (lengthSquared == 1)
|
||||||
|
{
|
||||||
|
result.X = X;
|
||||||
|
result.Y = Y;
|
||||||
|
}
|
||||||
|
else if (lengthSquared == 0)
|
||||||
|
{
|
||||||
|
result.X = 0;
|
||||||
|
result.Y = 0;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
double length = System.Math.Sqrt(lengthSquared);
|
||||||
|
result.X = X / length;
|
||||||
|
result.Y = Y / length;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
public static void Normalize(ref Vector2d vector, out Vector2d result)
|
||||||
|
{
|
||||||
|
double lengthSquared = vector.LengthSquared;
|
||||||
|
|
||||||
|
if (lengthSquared == 1)
|
||||||
|
{
|
||||||
|
result.X = vector.X;
|
||||||
|
result.Y = vector.Y;
|
||||||
|
}
|
||||||
|
else if (lengthSquared == 0)
|
||||||
|
{
|
||||||
|
result.X = 0;
|
||||||
|
result.Y = 0;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
double length = System.Math.Sqrt(lengthSquared);
|
||||||
|
result.X = vector.X / length;
|
||||||
|
result.Y = vector.Y / length;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
public void NormalizeApprox()
|
||||||
|
{
|
||||||
|
double inverseSquare = Functions.InverseSqrtFast(X * X + Y * Y);
|
||||||
|
X = X * inverseSquare;
|
||||||
|
Y = Y * inverseSquare;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>Gets left approximately normalized vector of the vector.</summary>
|
||||||
|
public void NormalizedApprox(out Vector2d result)
|
||||||
|
{
|
||||||
|
double inverseSquare = Functions.InverseSqrtFast(X * X + Y * Y);
|
||||||
|
result.X = X * inverseSquare;
|
||||||
|
result.Y = Y * inverseSquare;
|
||||||
|
}
|
||||||
|
|
||||||
|
public static void NormalizeApprox(ref Vector2d vector, out Vector2d result)
|
||||||
|
{
|
||||||
|
double inverseSquare = Functions.InverseSqrtFast(vector.X * vector.X + vector.Y * vector.Y);
|
||||||
|
result.X = vector.X * inverseSquare;
|
||||||
|
result.Y = vector.Y * inverseSquare;
|
||||||
|
}
|
||||||
|
|
||||||
|
#endregion
|
||||||
|
|
||||||
|
|
||||||
|
/// <summary>Gets the dot product of two vectors.</summary>
|
||||||
|
/// <param name="matrix">The first vector.</param>
|
||||||
|
/// <param name="right">The second vector.</param>
|
||||||
|
/// <returns>The dot product of two vectors.</returns>
|
||||||
|
public static double DotProduct(ref Vector2d left, ref Vector2d right)
|
||||||
|
{
|
||||||
|
return left.X * right.X + left.Y * right.Y;
|
||||||
|
}
|
||||||
|
|
||||||
|
#region Abs
|
||||||
|
|
||||||
|
public void Abs()
|
||||||
|
{
|
||||||
|
X = System.Math.Abs(X);
|
||||||
|
Y = System.Math.Abs(Y);
|
||||||
|
}
|
||||||
|
public void Abs(out Vector2d result)
|
||||||
|
{
|
||||||
|
result.X = System.Math.Abs(X);
|
||||||
|
result.Y = System.Math.Abs(Y);
|
||||||
|
}
|
||||||
|
public static void Abs(ref Vector2d vector, out Vector2d result)
|
||||||
|
{
|
||||||
|
result.X = System.Math.Abs(vector.X);
|
||||||
|
result.Y = System.Math.Abs(vector.Y);
|
||||||
|
}
|
||||||
|
|
||||||
|
#endregion
|
||||||
|
|
||||||
|
|
||||||
|
#region Inverse
|
||||||
|
|
||||||
|
public void Inverse()
|
||||||
|
{
|
||||||
|
X = -X;
|
||||||
|
Y = -Y;
|
||||||
|
}
|
||||||
|
public void Inverse(out Vector2d result)
|
||||||
|
{
|
||||||
|
result.X = -X;
|
||||||
|
result.Y = -Y;
|
||||||
|
}
|
||||||
|
public static void Inverse(ref Vector2d vector, out Vector2d result)
|
||||||
|
{
|
||||||
|
result.X = -vector.X;
|
||||||
|
result.Y = -vector.Y;
|
||||||
|
}
|
||||||
|
|
||||||
|
#endregion
|
||||||
|
|
||||||
|
|
||||||
|
#region Arithmatic
|
||||||
|
|
||||||
|
public void Add(ref Vector2d vector)
|
||||||
|
{
|
||||||
|
X = X + vector.X;
|
||||||
|
Y = Y + vector.Y;
|
||||||
|
}
|
||||||
|
public void Add(ref Vector2d vector, out Vector2d result)
|
||||||
|
{
|
||||||
|
result.X = X + vector.X;
|
||||||
|
result.Y = Y + vector.Y;
|
||||||
|
}
|
||||||
|
public static void Add(ref Vector2d left, ref Vector2d right, out Vector2d result)
|
||||||
|
{
|
||||||
|
result.X = left.X + right.X;
|
||||||
|
result.Y = left.Y + right.Y;
|
||||||
|
}
|
||||||
|
|
||||||
|
public void Add(double x, double y)
|
||||||
|
{
|
||||||
|
X = X + x;
|
||||||
|
Y = Y + y;
|
||||||
|
}
|
||||||
|
public void Add(double x, double y, out Vector2d result)
|
||||||
|
{
|
||||||
|
result.X = X + x;
|
||||||
|
result.Y = Y + y;
|
||||||
|
}
|
||||||
|
public static void Add(ref Vector2d vector, double x, double y, out Vector2d result)
|
||||||
|
{
|
||||||
|
result.X = vector.X + x;
|
||||||
|
result.Y = vector.Y + y;
|
||||||
|
}
|
||||||
|
|
||||||
|
public void Subtract(ref Vector2d vector)
|
||||||
|
{
|
||||||
|
X = X - vector.X;
|
||||||
|
Y = Y - vector.Y;
|
||||||
|
}
|
||||||
|
public void Subtract(ref Vector2d vector, out Vector2d result)
|
||||||
|
{
|
||||||
|
result.X = X - vector.X;
|
||||||
|
result.Y = Y - vector.Y;
|
||||||
|
}
|
||||||
|
public static void Subtract(ref Vector2d left, ref Vector2d right, out Vector2d result)
|
||||||
|
{
|
||||||
|
result.X = left.X - right.X;
|
||||||
|
result.Y = left.Y - right.Y;
|
||||||
|
}
|
||||||
|
|
||||||
|
public void Subtract(double x, double y)
|
||||||
|
{
|
||||||
|
X = X - x;
|
||||||
|
Y = Y - y;
|
||||||
|
}
|
||||||
|
public void Subtract(double x, double y, out Vector2d result)
|
||||||
|
{
|
||||||
|
result.X = X - x;
|
||||||
|
result.Y = Y - y;
|
||||||
|
}
|
||||||
|
public static void Subtract(ref Vector2d vector, double x, double y, out Vector2d result)
|
||||||
|
{
|
||||||
|
result.X = vector.X - x;
|
||||||
|
result.Y = vector.Y - y;
|
||||||
|
}
|
||||||
|
|
||||||
|
public void Multiply(double scalar)
|
||||||
|
{
|
||||||
|
X = X * scalar;
|
||||||
|
Y = Y * scalar;
|
||||||
|
}
|
||||||
|
public void Multiply(double scalar, out Vector2d result)
|
||||||
|
{
|
||||||
|
result.X = X * scalar;
|
||||||
|
result.Y = Y * scalar;
|
||||||
|
}
|
||||||
|
public static void Multiply(ref Vector2d vector, double scalar, out Vector2d result)
|
||||||
|
{
|
||||||
|
result.X = vector.X * scalar;
|
||||||
|
result.Y = vector.Y * scalar;
|
||||||
|
}
|
||||||
|
|
||||||
|
public void Multiply(ref Vector2d vector)
|
||||||
|
{
|
||||||
|
X = X * vector.X;
|
||||||
|
Y = Y * vector.Y;
|
||||||
|
}
|
||||||
|
public void Multiply(ref Vector2d vector, out Vector2d result)
|
||||||
|
{
|
||||||
|
result.X = X * vector.X;
|
||||||
|
result.Y = Y * vector.Y;
|
||||||
|
}
|
||||||
|
public static void Multiply(ref Vector2d left, ref Vector2d right, out Vector2d result)
|
||||||
|
{
|
||||||
|
result.X = left.X * right.X;
|
||||||
|
result.Y = left.Y * right.Y;
|
||||||
|
}
|
||||||
|
|
||||||
|
public void Multiply(double x, double y)
|
||||||
|
{
|
||||||
|
X = X * x;
|
||||||
|
Y = Y * y;
|
||||||
|
}
|
||||||
|
public void Multiply(double x, double y, out Vector2d result)
|
||||||
|
{
|
||||||
|
result.X = X * x;
|
||||||
|
result.Y = Y * y;
|
||||||
|
}
|
||||||
|
public static void Multiply(ref Vector2d vector, double x, double y, out Vector2d result)
|
||||||
|
{
|
||||||
|
result.X = vector.X * x;
|
||||||
|
result.Y = vector.Y * y;
|
||||||
|
}
|
||||||
|
|
||||||
|
public void Divide(double scalar)
|
||||||
|
{
|
||||||
|
X = X / scalar;
|
||||||
|
Y = Y / scalar;
|
||||||
|
}
|
||||||
|
public void Divide(double scalar, out Vector2d result)
|
||||||
|
{
|
||||||
|
result.X = X / scalar;
|
||||||
|
result.Y = Y / scalar;
|
||||||
|
}
|
||||||
|
public static void Divide(ref Vector2d vector, double scalar, out Vector2d result)
|
||||||
|
{
|
||||||
|
result.X = vector.X / scalar;
|
||||||
|
result.Y = vector.Y / scalar;
|
||||||
|
}
|
||||||
|
|
||||||
|
public void Divide(ref Vector2d vector)
|
||||||
|
{
|
||||||
|
X = X / vector.X;
|
||||||
|
Y = Y / vector.Y;
|
||||||
|
}
|
||||||
|
public void Divide(ref Vector2d vector, out Vector2d result)
|
||||||
|
{
|
||||||
|
result.X = X / vector.X;
|
||||||
|
result.Y = Y / vector.Y;
|
||||||
|
}
|
||||||
|
public static void Divide(ref Vector2d left, ref Vector2d right, out Vector2d result)
|
||||||
|
{
|
||||||
|
result.X = left.X / right.X;
|
||||||
|
result.Y = left.Y / right.Y;
|
||||||
|
}
|
||||||
|
|
||||||
|
public void Divide(double x, double y)
|
||||||
|
{
|
||||||
|
X = X / x;
|
||||||
|
Y = Y / y;
|
||||||
|
}
|
||||||
|
public void Divide(double x, double y, out Vector2d result)
|
||||||
|
{
|
||||||
|
result.X = X / x;
|
||||||
|
result.Y = Y / y;
|
||||||
|
}
|
||||||
|
public static void Divide(ref Vector2d vector, double x, double y, out Vector2d result)
|
||||||
|
{
|
||||||
|
result.X = vector.X / x;
|
||||||
|
result.Y = vector.Y / y;
|
||||||
|
}
|
||||||
|
|
||||||
|
#endregion
|
||||||
|
|
||||||
|
|
||||||
|
#region Transformations
|
||||||
|
|
||||||
|
public void Transform(ref Matrix4d matrix)
|
||||||
|
{
|
||||||
|
double x = matrix.R0C0 * X + matrix.R0C1 * Y;
|
||||||
|
Y = matrix.R1C0 * X + matrix.R1C1 * Y;
|
||||||
|
X = x;
|
||||||
|
}
|
||||||
|
public void Transform(ref Matrix4d matrix, out Vector2d result)
|
||||||
|
{
|
||||||
|
result.X = matrix.R0C0 * X + matrix.R0C1 * Y;
|
||||||
|
result.Y = matrix.R1C0 * X + matrix.R1C1 * Y;
|
||||||
|
}
|
||||||
|
public static void Transform(ref Vector2d vector, ref Matrix4d matrix, out Vector2d result)
|
||||||
|
{
|
||||||
|
result.X = matrix.R0C0 * vector.X + matrix.R0C1 * vector.Y;
|
||||||
|
result.Y = matrix.R1C0 * vector.X + matrix.R1C1 * vector.Y;
|
||||||
|
}
|
||||||
|
|
||||||
|
public void Translate(ref Vector2d vector)
|
||||||
|
{
|
||||||
|
X = X + vector.X;
|
||||||
|
Y = Y + vector.Y;
|
||||||
|
}
|
||||||
|
public void Translate(ref Vector2d vector, out Vector2d result)
|
||||||
|
{
|
||||||
|
result.X = X + vector.X;
|
||||||
|
result.Y = Y + vector.Y;
|
||||||
|
}
|
||||||
|
public static void Translate(ref Vector2d left, ref Vector2d right, out Vector2d result)
|
||||||
|
{
|
||||||
|
result.X = left.X + right.X;
|
||||||
|
result.Y = left.Y + right.Y;
|
||||||
|
}
|
||||||
|
|
||||||
|
public void Translate(double x, double y)
|
||||||
|
{
|
||||||
|
X = X + x;
|
||||||
|
Y = Y + y;
|
||||||
|
}
|
||||||
|
public void Translate(double x, double y, out Vector2d result)
|
||||||
|
{
|
||||||
|
result.X = X + x;
|
||||||
|
result.Y = Y + y;
|
||||||
|
}
|
||||||
|
public static void Translate(ref Vector2d vector, double x, double y, out Vector2d result)
|
||||||
|
{
|
||||||
|
result.X = vector.X + x;
|
||||||
|
result.Y = vector.Y + y;
|
||||||
|
}
|
||||||
|
|
||||||
|
public void Scale(ref Vector2d vector)
|
||||||
|
{
|
||||||
|
X = X * vector.X;
|
||||||
|
Y = Y * vector.Y;
|
||||||
|
}
|
||||||
|
public void Scale(ref Vector2d vector, out Vector2d result)
|
||||||
|
{
|
||||||
|
result.X = X * vector.X;
|
||||||
|
result.Y = Y * vector.Y;
|
||||||
|
}
|
||||||
|
public static void Scale(ref Vector2d left, ref Vector2d right, out Vector2d result)
|
||||||
|
{
|
||||||
|
result.X = left.X * right.X;
|
||||||
|
result.Y = left.Y * right.Y;
|
||||||
|
}
|
||||||
|
|
||||||
|
public void Scale(double x, double y)
|
||||||
|
{
|
||||||
|
X = X * x;
|
||||||
|
Y = Y * y;
|
||||||
|
}
|
||||||
|
public void Scale(double x, double y, out Vector2d result)
|
||||||
|
{
|
||||||
|
result.X = X * x;
|
||||||
|
result.Y = Y * y;
|
||||||
|
}
|
||||||
|
public static void Scale(ref Vector2d vector, double x, double y, out Vector2d result)
|
||||||
|
{
|
||||||
|
result.X = vector.X * x;
|
||||||
|
result.Y = vector.Y * y;
|
||||||
|
}
|
||||||
|
|
||||||
|
public void Rotate(double angle)
|
||||||
|
{
|
||||||
|
double angleRadians = Functions.DTOR * angle;
|
||||||
|
double sin = (double)System.Math.Sin(angleRadians);
|
||||||
|
double cos = (double)System.Math.Cos(angleRadians);
|
||||||
|
|
||||||
|
double x = cos * X + sin * Y;
|
||||||
|
Y = cos * Y - sin * X;
|
||||||
|
X = x;
|
||||||
|
}
|
||||||
|
public void Rotate(double angle, out Vector2d result)
|
||||||
|
{
|
||||||
|
double angleRadians = Functions.DTOR * angle;
|
||||||
|
double sin = (double)System.Math.Sin(angleRadians);
|
||||||
|
double cos = (double)System.Math.Cos(angleRadians);
|
||||||
|
|
||||||
|
result.X = cos * X + sin * Y;
|
||||||
|
result.Y = cos * Y - sin * X;
|
||||||
|
}
|
||||||
|
public static void Rotate(ref Vector2d vector, double angle, out Vector2d result)
|
||||||
|
{
|
||||||
|
double angleRadians = Functions.DTOR * angle;
|
||||||
|
double sin = (double)System.Math.Sin(angleRadians);
|
||||||
|
double cos = (double)System.Math.Cos(angleRadians);
|
||||||
|
|
||||||
|
result.X = cos * vector.X + sin * vector.Y;
|
||||||
|
result.Y = cos * vector.Y - sin * vector.X;
|
||||||
|
}
|
||||||
|
|
||||||
|
#endregion
|
||||||
|
|
||||||
|
|
||||||
|
#region Min & Max
|
||||||
|
|
||||||
|
public void Min(ref Vector2d vector)
|
||||||
|
{
|
||||||
|
double lengthSquared = X * X + Y * Y;
|
||||||
|
double vectorLengthSquared = vector.X * vector.X + vector.Y * vector.Y;
|
||||||
|
|
||||||
|
if (vectorLengthSquared < lengthSquared)
|
||||||
|
{
|
||||||
|
X = vector.X;
|
||||||
|
Y = vector.Y;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
public void Min(ref Vector2d vector, out Vector2d result)
|
||||||
|
{
|
||||||
|
double lengthSquared = X * X + Y * Y;
|
||||||
|
double vectorLengthSquared = vector.X * vector.X + vector.Y * vector.Y;
|
||||||
|
|
||||||
|
if (vectorLengthSquared < lengthSquared)
|
||||||
|
{
|
||||||
|
result.X = vector.X;
|
||||||
|
result.Y = vector.Y;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
result.X = X;
|
||||||
|
result.Y = Y;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
public static void Min(ref Vector2d left, ref Vector2d right, out Vector2d result)
|
||||||
|
{
|
||||||
|
double leftLengthSquared = left.X * left.X + left.Y * left.Y;
|
||||||
|
double rightLengthSquared = right.X * right.X + right.Y * right.Y;
|
||||||
|
|
||||||
|
if (rightLengthSquared < leftLengthSquared)
|
||||||
|
{
|
||||||
|
result.X = right.X;
|
||||||
|
result.Y = right.Y;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
result.X = left.X;
|
||||||
|
result.Y = left.Y;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
public void Max(ref Vector2d vector)
|
||||||
|
{
|
||||||
|
double lengthSquared = X * X + Y * Y;
|
||||||
|
double vectorLengthSquared = vector.X * vector.X + vector.Y * vector.Y;
|
||||||
|
|
||||||
|
if (vectorLengthSquared > lengthSquared)
|
||||||
|
{
|
||||||
|
X = vector.X;
|
||||||
|
Y = vector.Y;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
public void Max(ref Vector2d vector, out Vector2d result)
|
||||||
|
{
|
||||||
|
double lengthSquared = X * X + Y * Y;
|
||||||
|
double vectorLengthSquared = vector.X * vector.X + vector.Y * vector.Y;
|
||||||
|
|
||||||
|
if (vectorLengthSquared > lengthSquared)
|
||||||
|
{
|
||||||
|
result.X = vector.X;
|
||||||
|
result.Y = vector.Y;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
result.X = X;
|
||||||
|
result.Y = Y;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
public static void Max(ref Vector2d left, ref Vector2d right, out Vector2d result)
|
||||||
|
{
|
||||||
|
double leftLengthSquared = left.X * left.X + left.Y * left.Y;
|
||||||
|
double rightLengthSquared = right.X * right.X + right.Y * right.Y;
|
||||||
|
|
||||||
|
if (rightLengthSquared > leftLengthSquared)
|
||||||
|
{
|
||||||
|
result.X = right.X;
|
||||||
|
result.Y = right.Y;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
result.X = left.X;
|
||||||
|
result.Y = left.Y;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
public void CoordinateMin(ref Vector2d vector)
|
||||||
|
{
|
||||||
|
X = System.Math.Min(X, vector.X);
|
||||||
|
Y = System.Math.Min(Y, vector.Y);
|
||||||
|
}
|
||||||
|
public void CoordinateMin(ref Vector2d vector, out Vector2d result)
|
||||||
|
{
|
||||||
|
result.X = System.Math.Min(X, vector.X);
|
||||||
|
result.Y = System.Math.Min(Y, vector.Y);
|
||||||
|
}
|
||||||
|
public static void CoordinateMin(ref Vector2d left, ref Vector2d right, out Vector2d result)
|
||||||
|
{
|
||||||
|
result.X = System.Math.Min(left.X, right.X);
|
||||||
|
result.Y = System.Math.Min(left.Y, right.Y);
|
||||||
|
}
|
||||||
|
|
||||||
|
public void CoordinateMax(ref Vector2d vector)
|
||||||
|
{
|
||||||
|
X = System.Math.Max(X, vector.X);
|
||||||
|
Y = System.Math.Max(Y, vector.Y);
|
||||||
|
}
|
||||||
|
public void CoordinateMax(ref Vector2d vector, out Vector2d result)
|
||||||
|
{
|
||||||
|
result.X = System.Math.Max(X, vector.X);
|
||||||
|
result.Y = System.Math.Max(Y, vector.Y);
|
||||||
|
}
|
||||||
|
public static void CoordinateMax(ref Vector2d left, ref Vector2d right, out Vector2d result)
|
||||||
|
{
|
||||||
|
result.X = System.Math.Max(left.X, right.X);
|
||||||
|
result.Y = System.Math.Max(left.Y, right.Y);
|
||||||
|
}
|
||||||
|
|
||||||
|
public void Clamp(ref Vector2d min, ref Vector2d max)
|
||||||
|
{
|
||||||
|
X = System.Math.Max(System.Math.Min(X, min.X), max.X);
|
||||||
|
Y = System.Math.Max(System.Math.Min(Y, min.Y), max.Y);
|
||||||
|
}
|
||||||
|
public void Clamp(ref Vector2d min, ref Vector2d max, out Vector2d result)
|
||||||
|
{
|
||||||
|
result.X = System.Math.Max(System.Math.Min(X, min.X), max.X);
|
||||||
|
result.Y = System.Math.Max(System.Math.Min(Y, min.Y), max.Y);
|
||||||
|
}
|
||||||
|
public static void Clamp(ref Vector2d vector, ref Vector2d min, ref Vector2d max, out Vector2d result)
|
||||||
|
{
|
||||||
|
result.X = System.Math.Max(System.Math.Min(vector.X, min.X), max.X);
|
||||||
|
result.Y = System.Math.Max(System.Math.Min(vector.Y, min.Y), max.Y);
|
||||||
|
}
|
||||||
|
|
||||||
|
#endregion
|
||||||
|
|
||||||
|
|
||||||
|
#region Interpolation
|
||||||
|
|
||||||
|
public void Lerp(ref Vector2d end, double blend)
|
||||||
|
{
|
||||||
|
X = X + (end.X - X) * blend;
|
||||||
|
Y = Y + (end.Y - Y) * blend;
|
||||||
|
}
|
||||||
|
public void Lerp(ref Vector2d end, double blend, out Vector2d result)
|
||||||
|
{
|
||||||
|
result.X = X + (end.X - X) * blend;
|
||||||
|
result.Y = Y + (end.Y - Y) * blend;
|
||||||
|
}
|
||||||
|
public static void Lerp(ref Vector2d start, ref Vector2d end, double blend, out Vector2d result)
|
||||||
|
{
|
||||||
|
result.X = start.X + (end.X - start.X) * blend;
|
||||||
|
result.Y = start.Y + (end.Y - start.Y) * blend;
|
||||||
|
}
|
||||||
|
|
||||||
|
public void BaryCentric(ref Vector2d endU, ref Vector2d endV, double u, double v)
|
||||||
|
{
|
||||||
|
X = X + (endU.X - X) * u + (endV.X - X) * v;
|
||||||
|
Y = Y + (endU.Y - Y) * u + (endV.Y - Y) * v;
|
||||||
|
}
|
||||||
|
public void BaryCentric(ref Vector2d endU, ref Vector2d endV, double u, double v, out Vector2d result)
|
||||||
|
{
|
||||||
|
result.X = X + (endU.X - X) * u + (endV.X - X) * v;
|
||||||
|
result.Y = Y + (endU.Y - Y) * u + (endV.Y - Y) * v;
|
||||||
|
}
|
||||||
|
public static void BaryCentric(ref Vector2d start, ref Vector2d endU, ref Vector2d endV, double u, double v, out Vector2d result)
|
||||||
|
{
|
||||||
|
result.X = start.X + (endU.X - start.X) * u + (endV.X - start.X) * v;
|
||||||
|
result.Y = start.Y + (endU.Y - start.Y) * u + (endV.Y - start.Y) * v;
|
||||||
|
}
|
||||||
|
|
||||||
|
#endregion
|
||||||
|
|
||||||
|
|
||||||
|
#region String and Parse
|
||||||
|
|
||||||
|
/// <summary>Returns the fully qualified type name of this instance.</summary>
|
||||||
|
/// <returns>A System.String containing left fully qualified type name.</returns>
|
||||||
|
public override string ToString()
|
||||||
|
{
|
||||||
|
return String.Format("{0} {1} {2}", X, Y);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>Parse left string to convert it to left vector.</summary>
|
||||||
|
/// <param name="str">The string to parse.</param>
|
||||||
|
/// <returns>The vector represented by the string.</returns>
|
||||||
|
public static void Parse(string str, out Vector2d result)
|
||||||
|
{
|
||||||
|
Match match = new Regex(@"(?<x>.*) (?<y>.*)", RegexOptions.None).Match(str);
|
||||||
|
if (!match.Success) throw new Exception("Parse failed!");
|
||||||
|
result.X = double.Parse(match.Result("${x}"));
|
||||||
|
result.Y = double.Parse(match.Result("${y}"));
|
||||||
|
}
|
||||||
|
|
||||||
|
#endregion
|
||||||
|
|
||||||
|
|
||||||
|
#region HashCode
|
||||||
|
|
||||||
|
/// <summary>Returns the hash code for this instance.</summary>
|
||||||
|
/// <returns>A 32-bit signed integer that is the hash code for this instance.</returns>
|
||||||
|
public override int GetHashCode()
|
||||||
|
{
|
||||||
|
return X.GetHashCode() ^ Y.GetHashCode();
|
||||||
|
}
|
||||||
|
|
||||||
|
#endregion
|
||||||
|
|
||||||
|
|
||||||
|
#region Constants
|
||||||
|
|
||||||
|
/// <summary>A vector representing left zero vector.</summary>
|
||||||
|
public static readonly Vector2d Zero = new Vector2d(0, 0);
|
||||||
|
|
||||||
|
/// <summary>A vector with all coordinates set to one.</summary>
|
||||||
|
public static readonly Vector2d One = new Vector2d(1, 1);
|
||||||
|
|
||||||
|
#endregion
|
||||||
|
}
|
||||||
|
}
|
||||||
1216
Source/OpenTK/Math/Vector3d.cs
Normal file
1216
Source/OpenTK/Math/Vector3d.cs
Normal file
File diff suppressed because it is too large
Load diff
1343
Source/OpenTK/Math/Vector4d.cs
Normal file
1343
Source/OpenTK/Math/Vector4d.cs
Normal file
File diff suppressed because it is too large
Load diff
Loading…
Reference in a new issue