2017-10-02 23:24:00 +02:00
|
|
|
|
using System;
|
|
|
|
|
using System.Runtime.InteropServices;
|
2018-03-01 08:51:35 +01:00
|
|
|
|
#if REAL_T_IS_DOUBLE
|
|
|
|
|
using real_t = System.Double;
|
|
|
|
|
#else
|
|
|
|
|
using real_t = System.Single;
|
|
|
|
|
#endif
|
|
|
|
|
|
2017-10-02 23:24:00 +02:00
|
|
|
|
namespace Godot
|
|
|
|
|
{
|
2020-07-07 23:29:35 +02:00
|
|
|
|
/// <summary>
|
|
|
|
|
/// 3×3 matrix used for 3D rotation and scale.
|
|
|
|
|
/// Almost always used as an orthogonal basis for a Transform.
|
|
|
|
|
///
|
|
|
|
|
/// Contains 3 vector fields X, Y and Z as its columns, which are typically
|
|
|
|
|
/// interpreted as the local basis vectors of a 3D transformation. For such use,
|
|
|
|
|
/// it is composed of a scaling and a rotation matrix, in that order (M = R.S).
|
|
|
|
|
///
|
|
|
|
|
/// Can also be accessed as array of 3D vectors. These vectors are normally
|
|
|
|
|
/// orthogonal to each other, but are not necessarily normalized (due to scaling).
|
|
|
|
|
///
|
|
|
|
|
/// For more information, read this documentation article:
|
|
|
|
|
/// https://docs.godotengine.org/en/latest/tutorials/math/matrices_and_transforms.html
|
|
|
|
|
/// </summary>
|
2019-08-08 03:29:40 +02:00
|
|
|
|
[Serializable]
|
2017-10-02 23:24:00 +02:00
|
|
|
|
[StructLayout(LayoutKind.Sequential)]
|
|
|
|
|
public struct Basis : IEquatable<Basis>
|
|
|
|
|
{
|
2019-02-23 18:22:27 +01:00
|
|
|
|
// NOTE: x, y and z are public-only. Use Column0, Column1 and Column2 internally.
|
|
|
|
|
|
|
|
|
|
/// <summary>
|
2020-07-07 23:29:35 +02:00
|
|
|
|
/// The basis matrix's X vector (column 0).
|
2019-02-23 18:22:27 +01:00
|
|
|
|
/// </summary>
|
2020-07-07 23:29:35 +02:00
|
|
|
|
/// <value>Equivalent to <see cref="Column0"/> and array index `[0]`.</value>
|
2018-03-08 03:49:28 +01:00
|
|
|
|
public Vector3 x
|
|
|
|
|
{
|
2019-02-23 18:22:27 +01:00
|
|
|
|
get => Column0;
|
|
|
|
|
set => Column0 = value;
|
2018-03-08 03:49:28 +01:00
|
|
|
|
}
|
|
|
|
|
|
2019-02-23 18:22:27 +01:00
|
|
|
|
/// <summary>
|
2020-07-07 23:29:35 +02:00
|
|
|
|
/// The basis matrix's Y vector (column 1).
|
2019-02-23 18:22:27 +01:00
|
|
|
|
/// </summary>
|
2020-07-07 23:29:35 +02:00
|
|
|
|
/// <value>Equivalent to <see cref="Column1"/> and array index `[1]`.</value>
|
2018-03-08 03:49:28 +01:00
|
|
|
|
public Vector3 y
|
|
|
|
|
{
|
2019-02-23 18:22:27 +01:00
|
|
|
|
get => Column1;
|
|
|
|
|
set => Column1 = value;
|
2018-03-08 03:49:28 +01:00
|
|
|
|
}
|
|
|
|
|
|
2019-02-23 18:22:27 +01:00
|
|
|
|
/// <summary>
|
2020-07-07 23:29:35 +02:00
|
|
|
|
/// The basis matrix's Z vector (column 2).
|
2019-02-23 18:22:27 +01:00
|
|
|
|
/// </summary>
|
2020-07-07 23:29:35 +02:00
|
|
|
|
/// <value>Equivalent to <see cref="Column2"/> and array index `[2]`.</value>
|
2018-03-08 03:49:28 +01:00
|
|
|
|
public Vector3 z
|
|
|
|
|
{
|
2019-02-23 18:22:27 +01:00
|
|
|
|
get => Column2;
|
|
|
|
|
set => Column2 = value;
|
2018-03-08 03:49:28 +01:00
|
|
|
|
}
|
|
|
|
|
|
2020-07-07 23:29:35 +02:00
|
|
|
|
/// <summary>
|
|
|
|
|
/// Row 0 of the basis matrix. Shows which vectors contribute
|
|
|
|
|
/// to the X direction. Rows are not very useful for user code,
|
|
|
|
|
/// but are more efficient for some internal calculations.
|
|
|
|
|
/// </summary>
|
2019-02-23 18:22:27 +01:00
|
|
|
|
public Vector3 Row0;
|
2020-07-07 23:29:35 +02:00
|
|
|
|
|
|
|
|
|
/// <summary>
|
|
|
|
|
/// Row 1 of the basis matrix. Shows which vectors contribute
|
|
|
|
|
/// to the Y direction. Rows are not very useful for user code,
|
|
|
|
|
/// but are more efficient for some internal calculations.
|
|
|
|
|
/// </summary>
|
2019-02-23 18:22:27 +01:00
|
|
|
|
public Vector3 Row1;
|
2020-07-07 23:29:35 +02:00
|
|
|
|
|
|
|
|
|
/// <summary>
|
|
|
|
|
/// Row 2 of the basis matrix. Shows which vectors contribute
|
|
|
|
|
/// to the Z direction. Rows are not very useful for user code,
|
|
|
|
|
/// but are more efficient for some internal calculations.
|
|
|
|
|
/// </summary>
|
2019-02-23 18:22:27 +01:00
|
|
|
|
public Vector3 Row2;
|
2017-10-02 23:24:00 +02:00
|
|
|
|
|
2020-07-07 23:29:35 +02:00
|
|
|
|
/// <summary>
|
|
|
|
|
/// Column 0 of the basis matrix (the X vector).
|
|
|
|
|
/// </summary>
|
|
|
|
|
/// <value>Equivalent to <see cref="x"/> and array index `[0]`.</value>
|
2019-02-23 18:22:27 +01:00
|
|
|
|
public Vector3 Column0
|
|
|
|
|
{
|
|
|
|
|
get => new Vector3(Row0.x, Row1.x, Row2.x);
|
|
|
|
|
set
|
|
|
|
|
{
|
|
|
|
|
this.Row0.x = value.x;
|
|
|
|
|
this.Row1.x = value.y;
|
|
|
|
|
this.Row2.x = value.z;
|
|
|
|
|
}
|
|
|
|
|
}
|
2020-07-07 23:29:35 +02:00
|
|
|
|
|
|
|
|
|
/// <summary>
|
|
|
|
|
/// Column 1 of the basis matrix (the Y vector).
|
|
|
|
|
/// </summary>
|
|
|
|
|
/// <value>Equivalent to <see cref="y"/> and array index `[1]`.</value>
|
2019-02-23 18:22:27 +01:00
|
|
|
|
public Vector3 Column1
|
2017-10-02 23:24:00 +02:00
|
|
|
|
{
|
2019-02-23 18:22:27 +01:00
|
|
|
|
get => new Vector3(Row0.y, Row1.y, Row2.y);
|
|
|
|
|
set
|
|
|
|
|
{
|
|
|
|
|
this.Row0.y = value.x;
|
|
|
|
|
this.Row1.y = value.y;
|
|
|
|
|
this.Row2.y = value.z;
|
|
|
|
|
}
|
|
|
|
|
}
|
2020-07-07 23:29:35 +02:00
|
|
|
|
|
|
|
|
|
/// <summary>
|
|
|
|
|
/// Column 2 of the basis matrix (the Z vector).
|
|
|
|
|
/// </summary>
|
|
|
|
|
/// <value>Equivalent to <see cref="z"/> and array index `[2]`.</value>
|
2019-02-23 18:22:27 +01:00
|
|
|
|
public Vector3 Column2
|
|
|
|
|
{
|
|
|
|
|
get => new Vector3(Row0.z, Row1.z, Row2.z);
|
|
|
|
|
set
|
|
|
|
|
{
|
|
|
|
|
this.Row0.z = value.x;
|
|
|
|
|
this.Row1.z = value.y;
|
|
|
|
|
this.Row2.z = value.z;
|
|
|
|
|
}
|
2017-10-02 23:24:00 +02:00
|
|
|
|
}
|
|
|
|
|
|
2020-07-07 23:29:35 +02:00
|
|
|
|
/// <summary>
|
|
|
|
|
/// The scale of this basis.
|
|
|
|
|
/// </summary>
|
|
|
|
|
/// <value>Equivalent to the lengths of each column vector, but negative if the determinant is negative.</value>
|
2017-10-02 23:24:00 +02:00
|
|
|
|
public Vector3 Scale
|
|
|
|
|
{
|
|
|
|
|
get
|
|
|
|
|
{
|
2019-02-23 18:22:27 +01:00
|
|
|
|
real_t detSign = Mathf.Sign(Determinant());
|
|
|
|
|
return detSign * new Vector3
|
2017-10-02 23:24:00 +02:00
|
|
|
|
(
|
2020-07-07 23:29:35 +02:00
|
|
|
|
Column0.Length(),
|
|
|
|
|
Column1.Length(),
|
|
|
|
|
Column2.Length()
|
2017-10-02 23:24:00 +02:00
|
|
|
|
);
|
|
|
|
|
}
|
2020-07-07 23:29:35 +02:00
|
|
|
|
set
|
|
|
|
|
{
|
|
|
|
|
value /= Scale; // Value becomes what's called "delta_scale" in core.
|
|
|
|
|
Column0 *= value.x;
|
|
|
|
|
Column1 *= value.y;
|
|
|
|
|
Column2 *= value.z;
|
|
|
|
|
}
|
2017-10-02 23:24:00 +02:00
|
|
|
|
}
|
|
|
|
|
|
2019-11-29 08:20:31 +01:00
|
|
|
|
/// <summary>
|
|
|
|
|
/// Access whole columns in the form of Vector3.
|
|
|
|
|
/// </summary>
|
|
|
|
|
/// <param name="column">Which column vector.</param>
|
|
|
|
|
public Vector3 this[int column]
|
2017-10-02 23:24:00 +02:00
|
|
|
|
{
|
|
|
|
|
get
|
|
|
|
|
{
|
2019-11-29 08:20:31 +01:00
|
|
|
|
switch (column)
|
2017-10-02 23:24:00 +02:00
|
|
|
|
{
|
|
|
|
|
case 0:
|
2019-02-23 18:22:27 +01:00
|
|
|
|
return Column0;
|
2017-10-02 23:24:00 +02:00
|
|
|
|
case 1:
|
2019-02-23 18:22:27 +01:00
|
|
|
|
return Column1;
|
2017-10-02 23:24:00 +02:00
|
|
|
|
case 2:
|
2019-02-23 18:22:27 +01:00
|
|
|
|
return Column2;
|
2017-10-02 23:24:00 +02:00
|
|
|
|
default:
|
|
|
|
|
throw new IndexOutOfRangeException();
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
set
|
|
|
|
|
{
|
2019-11-29 08:20:31 +01:00
|
|
|
|
switch (column)
|
2017-10-02 23:24:00 +02:00
|
|
|
|
{
|
|
|
|
|
case 0:
|
2019-02-23 18:22:27 +01:00
|
|
|
|
Column0 = value;
|
2017-10-02 23:24:00 +02:00
|
|
|
|
return;
|
|
|
|
|
case 1:
|
2019-02-23 18:22:27 +01:00
|
|
|
|
Column1 = value;
|
2017-10-02 23:24:00 +02:00
|
|
|
|
return;
|
|
|
|
|
case 2:
|
2019-02-23 18:22:27 +01:00
|
|
|
|
Column2 = value;
|
2017-10-02 23:24:00 +02:00
|
|
|
|
return;
|
|
|
|
|
default:
|
|
|
|
|
throw new IndexOutOfRangeException();
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
2019-11-29 08:20:31 +01:00
|
|
|
|
/// <summary>
|
|
|
|
|
/// Access matrix elements in column-major order.
|
|
|
|
|
/// </summary>
|
|
|
|
|
/// <param name="column">Which column, the matrix horizontal position.</param>
|
|
|
|
|
/// <param name="row">Which row, the matrix vertical position.</param>
|
|
|
|
|
public real_t this[int column, int row]
|
2017-10-02 23:24:00 +02:00
|
|
|
|
{
|
|
|
|
|
get
|
|
|
|
|
{
|
2019-11-29 08:20:31 +01:00
|
|
|
|
return this[column][row];
|
2017-10-02 23:24:00 +02:00
|
|
|
|
}
|
|
|
|
|
set
|
|
|
|
|
{
|
2019-11-29 08:20:31 +01:00
|
|
|
|
Vector3 columnVector = this[column];
|
|
|
|
|
columnVector[row] = value;
|
|
|
|
|
this[column] = columnVector;
|
2017-10-02 23:24:00 +02:00
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
2020-09-24 05:59:54 +02:00
|
|
|
|
public Quat RotationQuat()
|
2018-09-15 04:26:11 +02:00
|
|
|
|
{
|
|
|
|
|
Basis orthonormalizedBasis = Orthonormalized();
|
|
|
|
|
real_t det = orthonormalizedBasis.Determinant();
|
|
|
|
|
if (det < 0)
|
|
|
|
|
{
|
2020-07-07 23:29:35 +02:00
|
|
|
|
// Ensure that the determinant is 1, such that result is a proper
|
|
|
|
|
// rotation matrix which can be represented by Euler angles.
|
|
|
|
|
orthonormalizedBasis = orthonormalizedBasis.Scaled(-Vector3.One);
|
2018-09-15 04:26:11 +02:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
return orthonormalizedBasis.Quat();
|
|
|
|
|
}
|
|
|
|
|
|
2019-07-28 23:43:40 +02:00
|
|
|
|
internal void SetQuatScale(Quat quat, Vector3 scale)
|
2018-09-15 04:26:11 +02:00
|
|
|
|
{
|
|
|
|
|
SetDiagonal(scale);
|
|
|
|
|
Rotate(quat);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
private void Rotate(Quat quat)
|
|
|
|
|
{
|
|
|
|
|
this *= new Basis(quat);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
private void SetDiagonal(Vector3 diagonal)
|
|
|
|
|
{
|
2019-02-23 18:22:27 +01:00
|
|
|
|
Row0 = new Vector3(diagonal.x, 0, 0);
|
|
|
|
|
Row1 = new Vector3(0, diagonal.y, 0);
|
|
|
|
|
Row2 = new Vector3(0, 0, diagonal.z);
|
2018-09-15 04:26:11 +02:00
|
|
|
|
}
|
|
|
|
|
|
2020-07-07 23:29:35 +02:00
|
|
|
|
/// <summary>
|
|
|
|
|
/// Returns the determinant of the basis matrix. If the basis is
|
|
|
|
|
/// uniformly scaled, its determinant is the square of the scale.
|
|
|
|
|
///
|
|
|
|
|
/// A negative determinant means the basis has a negative scale.
|
|
|
|
|
/// A zero determinant means the basis isn't invertible,
|
|
|
|
|
/// and is usually considered invalid.
|
|
|
|
|
/// </summary>
|
|
|
|
|
/// <returns>The determinant of the basis matrix.</returns>
|
2018-03-01 08:51:35 +01:00
|
|
|
|
public real_t Determinant()
|
2017-10-02 23:24:00 +02:00
|
|
|
|
{
|
2019-02-23 18:22:27 +01:00
|
|
|
|
real_t cofac00 = Row1[1] * Row2[2] - Row1[2] * Row2[1];
|
|
|
|
|
real_t cofac10 = Row1[2] * Row2[0] - Row1[0] * Row2[2];
|
|
|
|
|
real_t cofac20 = Row1[0] * Row2[1] - Row1[1] * Row2[0];
|
2017-10-02 23:24:00 +02:00
|
|
|
|
|
2019-02-23 18:22:27 +01:00
|
|
|
|
return Row0[0] * cofac00 + Row0[1] * cofac10 + Row0[2] * cofac20;
|
2018-03-08 03:49:28 +01:00
|
|
|
|
}
|
|
|
|
|
|
2020-07-07 23:29:35 +02:00
|
|
|
|
/// <summary>
|
|
|
|
|
/// Returns the basis's rotation in the form of Euler angles
|
|
|
|
|
/// (in the YXZ convention: when *decomposing*, first Z, then X, and Y last).
|
|
|
|
|
/// The returned vector contains the rotation angles in
|
|
|
|
|
/// the format (X angle, Y angle, Z angle).
|
|
|
|
|
///
|
|
|
|
|
/// Consider using the <see cref="Basis.Quat()"/> method instead, which
|
|
|
|
|
/// returns a <see cref="Godot.Quat"/> quaternion instead of Euler angles.
|
|
|
|
|
/// </summary>
|
|
|
|
|
/// <returns>A Vector3 representing the basis rotation in Euler angles.</returns>
|
2017-11-21 23:32:19 +01:00
|
|
|
|
public Vector3 GetEuler()
|
2017-10-02 23:24:00 +02:00
|
|
|
|
{
|
2018-04-08 05:28:24 +02:00
|
|
|
|
Basis m = Orthonormalized();
|
2017-10-02 23:24:00 +02:00
|
|
|
|
|
|
|
|
|
Vector3 euler;
|
2017-10-09 14:49:53 +02:00
|
|
|
|
euler.z = 0.0f;
|
2017-10-02 23:24:00 +02:00
|
|
|
|
|
2018-11-22 08:19:19 +01:00
|
|
|
|
real_t mzy = m.Row1[2];
|
2017-10-09 14:49:53 +02:00
|
|
|
|
|
2018-11-22 08:19:19 +01:00
|
|
|
|
if (mzy < 1.0f)
|
2017-10-02 23:24:00 +02:00
|
|
|
|
{
|
2018-11-22 08:19:19 +01:00
|
|
|
|
if (mzy > -1.0f)
|
2017-10-02 23:24:00 +02:00
|
|
|
|
{
|
2018-11-22 08:19:19 +01:00
|
|
|
|
euler.x = Mathf.Asin(-mzy);
|
2019-02-23 18:22:27 +01:00
|
|
|
|
euler.y = Mathf.Atan2(m.Row0[2], m.Row2[2]);
|
|
|
|
|
euler.z = Mathf.Atan2(m.Row1[0], m.Row1[1]);
|
2017-10-02 23:24:00 +02:00
|
|
|
|
}
|
|
|
|
|
else
|
|
|
|
|
{
|
2018-04-07 13:54:07 +02:00
|
|
|
|
euler.x = Mathf.Pi * 0.5f;
|
2019-02-23 18:22:27 +01:00
|
|
|
|
euler.y = -Mathf.Atan2(-m.Row0[1], m.Row0[0]);
|
2017-10-02 23:24:00 +02:00
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
else
|
|
|
|
|
{
|
2018-04-07 13:54:07 +02:00
|
|
|
|
euler.x = -Mathf.Pi * 0.5f;
|
2019-02-23 18:22:27 +01:00
|
|
|
|
euler.y = -Mathf.Atan2(-m.Row0[1], m.Row0[0]);
|
2017-10-02 23:24:00 +02:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
return euler;
|
|
|
|
|
}
|
|
|
|
|
|
2020-07-07 23:29:35 +02:00
|
|
|
|
/// <summary>
|
|
|
|
|
/// Get rows by index. Rows are not very useful for user code,
|
|
|
|
|
/// but are more efficient for some internal calculations.
|
|
|
|
|
/// </summary>
|
|
|
|
|
/// <param name="index">Which row.</param>
|
|
|
|
|
/// <returns>One of `Row0`, `Row1`, or `Row2`.</returns>
|
2019-02-23 18:22:27 +01:00
|
|
|
|
public Vector3 GetRow(int index)
|
|
|
|
|
{
|
|
|
|
|
switch (index)
|
|
|
|
|
{
|
|
|
|
|
case 0:
|
|
|
|
|
return Row0;
|
|
|
|
|
case 1:
|
|
|
|
|
return Row1;
|
|
|
|
|
case 2:
|
|
|
|
|
return Row2;
|
|
|
|
|
default:
|
|
|
|
|
throw new IndexOutOfRangeException();
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
2020-07-07 23:29:35 +02:00
|
|
|
|
/// <summary>
|
|
|
|
|
/// Sets rows by index. Rows are not very useful for user code,
|
|
|
|
|
/// but are more efficient for some internal calculations.
|
|
|
|
|
/// </summary>
|
|
|
|
|
/// <param name="index">Which row.</param>
|
|
|
|
|
/// <param name="value">The vector to set the row to.</param>
|
2019-02-23 18:22:27 +01:00
|
|
|
|
public void SetRow(int index, Vector3 value)
|
|
|
|
|
{
|
|
|
|
|
switch (index)
|
|
|
|
|
{
|
|
|
|
|
case 0:
|
|
|
|
|
Row0 = value;
|
|
|
|
|
return;
|
|
|
|
|
case 1:
|
|
|
|
|
Row1 = value;
|
|
|
|
|
return;
|
|
|
|
|
case 2:
|
|
|
|
|
Row2 = value;
|
|
|
|
|
return;
|
|
|
|
|
default:
|
|
|
|
|
throw new IndexOutOfRangeException();
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
2020-07-07 23:29:35 +02:00
|
|
|
|
/// <summary>
|
|
|
|
|
/// This function considers a discretization of rotations into
|
|
|
|
|
/// 24 points on unit sphere, lying along the vectors (x, y, z) with
|
|
|
|
|
/// each component being either -1, 0, or 1, and returns the index
|
|
|
|
|
/// of the point best representing the orientation of the object.
|
|
|
|
|
/// It is mainly used by the <see cref="GridMap"/> editor.
|
|
|
|
|
///
|
|
|
|
|
/// For further details, refer to the Godot source code.
|
|
|
|
|
/// </summary>
|
|
|
|
|
/// <returns>The orthogonal index.</returns>
|
2017-11-21 23:32:19 +01:00
|
|
|
|
public int GetOrthogonalIndex()
|
2017-10-02 23:24:00 +02:00
|
|
|
|
{
|
2018-04-08 05:30:43 +02:00
|
|
|
|
var orth = this;
|
2017-10-02 23:24:00 +02:00
|
|
|
|
|
2018-04-17 00:33:42 +02:00
|
|
|
|
for (int i = 0; i < 3; i++)
|
2017-10-02 23:24:00 +02:00
|
|
|
|
{
|
2018-04-17 00:33:42 +02:00
|
|
|
|
for (int j = 0; j < 3; j++)
|
2017-10-02 23:24:00 +02:00
|
|
|
|
{
|
2019-02-23 18:22:27 +01:00
|
|
|
|
var row = orth.GetRow(i);
|
|
|
|
|
|
|
|
|
|
real_t v = row[j];
|
2017-10-02 23:24:00 +02:00
|
|
|
|
|
|
|
|
|
if (v > 0.5f)
|
2020-07-07 23:29:35 +02:00
|
|
|
|
{
|
2017-10-02 23:24:00 +02:00
|
|
|
|
v = 1.0f;
|
2020-07-07 23:29:35 +02:00
|
|
|
|
}
|
2017-10-02 23:24:00 +02:00
|
|
|
|
else if (v < -0.5f)
|
2020-07-07 23:29:35 +02:00
|
|
|
|
{
|
2017-10-02 23:24:00 +02:00
|
|
|
|
v = -1.0f;
|
2020-07-07 23:29:35 +02:00
|
|
|
|
}
|
2017-10-02 23:24:00 +02:00
|
|
|
|
else
|
2020-07-07 23:29:35 +02:00
|
|
|
|
{
|
2017-10-02 23:24:00 +02:00
|
|
|
|
v = 0f;
|
2020-07-07 23:29:35 +02:00
|
|
|
|
}
|
2017-10-02 23:24:00 +02:00
|
|
|
|
|
2019-02-23 18:22:27 +01:00
|
|
|
|
row[j] = v;
|
|
|
|
|
|
|
|
|
|
orth.SetRow(i, row);
|
2017-10-02 23:24:00 +02:00
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
2018-04-17 00:33:42 +02:00
|
|
|
|
for (int i = 0; i < 24; i++)
|
2017-10-02 23:24:00 +02:00
|
|
|
|
{
|
2019-08-09 05:29:45 +02:00
|
|
|
|
if (orth == _orthoBases[i])
|
2020-07-07 23:29:35 +02:00
|
|
|
|
{
|
2017-10-02 23:24:00 +02:00
|
|
|
|
return i;
|
2020-07-07 23:29:35 +02:00
|
|
|
|
}
|
2017-10-02 23:24:00 +02:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
return 0;
|
|
|
|
|
}
|
|
|
|
|
|
2020-07-07 23:29:35 +02:00
|
|
|
|
/// <summary>
|
|
|
|
|
/// Returns the inverse of the matrix.
|
|
|
|
|
/// </summary>
|
|
|
|
|
/// <returns>The inverse matrix.</returns>
|
2017-11-21 23:32:19 +01:00
|
|
|
|
public Basis Inverse()
|
2017-10-02 23:24:00 +02:00
|
|
|
|
{
|
2019-02-23 18:22:27 +01:00
|
|
|
|
real_t cofac00 = Row1[1] * Row2[2] - Row1[2] * Row2[1];
|
|
|
|
|
real_t cofac10 = Row1[2] * Row2[0] - Row1[0] * Row2[2];
|
|
|
|
|
real_t cofac20 = Row1[0] * Row2[1] - Row1[1] * Row2[0];
|
2017-10-02 23:24:00 +02:00
|
|
|
|
|
2019-02-23 18:22:27 +01:00
|
|
|
|
real_t det = Row0[0] * cofac00 + Row0[1] * cofac10 + Row0[2] * cofac20;
|
2017-10-02 23:24:00 +02:00
|
|
|
|
|
|
|
|
|
if (det == 0)
|
2020-07-07 23:29:35 +02:00
|
|
|
|
{
|
2019-02-23 18:22:27 +01:00
|
|
|
|
throw new InvalidOperationException("Matrix determinant is zero and cannot be inverted.");
|
2020-07-07 23:29:35 +02:00
|
|
|
|
}
|
2017-10-02 23:24:00 +02:00
|
|
|
|
|
2019-02-23 18:22:27 +01:00
|
|
|
|
real_t detInv = 1.0f / det;
|
2017-10-02 23:24:00 +02:00
|
|
|
|
|
2019-02-23 18:22:27 +01:00
|
|
|
|
real_t cofac01 = Row0[2] * Row2[1] - Row0[1] * Row2[2];
|
|
|
|
|
real_t cofac02 = Row0[1] * Row1[2] - Row0[2] * Row1[1];
|
|
|
|
|
real_t cofac11 = Row0[0] * Row2[2] - Row0[2] * Row2[0];
|
|
|
|
|
real_t cofac12 = Row0[2] * Row1[0] - Row0[0] * Row1[2];
|
|
|
|
|
real_t cofac21 = Row0[1] * Row2[0] - Row0[0] * Row2[1];
|
|
|
|
|
real_t cofac22 = Row0[0] * Row1[1] - Row0[1] * Row1[0];
|
|
|
|
|
|
|
|
|
|
return new Basis
|
2017-10-02 23:24:00 +02:00
|
|
|
|
(
|
2019-02-23 18:22:27 +01:00
|
|
|
|
cofac00 * detInv, cofac01 * detInv, cofac02 * detInv,
|
|
|
|
|
cofac10 * detInv, cofac11 * detInv, cofac12 * detInv,
|
|
|
|
|
cofac20 * detInv, cofac21 * detInv, cofac22 * detInv
|
2017-10-02 23:24:00 +02:00
|
|
|
|
);
|
|
|
|
|
}
|
|
|
|
|
|
2020-07-07 23:29:35 +02:00
|
|
|
|
/// <summary>
|
|
|
|
|
/// Returns the orthonormalized version of the basis matrix (useful to
|
|
|
|
|
/// call occasionally to avoid rounding errors for orthogonal matrices).
|
|
|
|
|
/// This performs a Gram-Schmidt orthonormalization on the basis of the matrix.
|
|
|
|
|
/// </summary>
|
|
|
|
|
/// <returns>An orthonormalized basis matrix.</returns>
|
2017-11-21 23:32:19 +01:00
|
|
|
|
public Basis Orthonormalized()
|
2017-10-02 23:24:00 +02:00
|
|
|
|
{
|
2020-07-07 23:29:35 +02:00
|
|
|
|
Vector3 column0 = this[0];
|
|
|
|
|
Vector3 column1 = this[1];
|
|
|
|
|
Vector3 column2 = this[2];
|
2017-10-02 23:24:00 +02:00
|
|
|
|
|
2019-02-23 18:22:27 +01:00
|
|
|
|
column0.Normalize();
|
|
|
|
|
column1 = column1 - column0 * column0.Dot(column1);
|
|
|
|
|
column1.Normalize();
|
|
|
|
|
column2 = column2 - column0 * column0.Dot(column2) - column1 * column1.Dot(column2);
|
|
|
|
|
column2.Normalize();
|
2017-10-02 23:24:00 +02:00
|
|
|
|
|
2019-02-23 18:22:27 +01:00
|
|
|
|
return new Basis(column0, column1, column2);
|
2017-10-02 23:24:00 +02:00
|
|
|
|
}
|
|
|
|
|
|
2020-07-07 23:29:35 +02:00
|
|
|
|
/// <summary>
|
|
|
|
|
/// Introduce an additional rotation around the given `axis`
|
|
|
|
|
/// by `phi` (in radians). The axis must be a normalized vector.
|
|
|
|
|
/// </summary>
|
|
|
|
|
/// <param name="axis">The axis to rotate around. Must be normalized.</param>
|
|
|
|
|
/// <param name="phi">The angle to rotate, in radians.</param>
|
|
|
|
|
/// <returns>The rotated basis matrix.</returns>
|
2018-03-01 08:51:35 +01:00
|
|
|
|
public Basis Rotated(Vector3 axis, real_t phi)
|
2017-10-02 23:24:00 +02:00
|
|
|
|
{
|
2017-10-09 14:49:53 +02:00
|
|
|
|
return new Basis(axis, phi) * this;
|
2017-10-02 23:24:00 +02:00
|
|
|
|
}
|
|
|
|
|
|
2020-07-07 23:29:35 +02:00
|
|
|
|
/// <summary>
|
|
|
|
|
/// Introduce an additional scaling specified by the given 3D scaling factor.
|
|
|
|
|
/// </summary>
|
|
|
|
|
/// <param name="scale">The scale to introduce.</param>
|
|
|
|
|
/// <returns>The scaled basis matrix.</returns>
|
2017-11-21 23:32:19 +01:00
|
|
|
|
public Basis Scaled(Vector3 scale)
|
2017-10-02 23:24:00 +02:00
|
|
|
|
{
|
2020-07-07 23:29:35 +02:00
|
|
|
|
Basis b = this;
|
2018-11-22 08:19:19 +01:00
|
|
|
|
b.Row0 *= scale.x;
|
|
|
|
|
b.Row1 *= scale.y;
|
|
|
|
|
b.Row2 *= scale.z;
|
|
|
|
|
return b;
|
2017-10-02 23:24:00 +02:00
|
|
|
|
}
|
|
|
|
|
|
2020-07-07 23:29:35 +02:00
|
|
|
|
/// <summary>
|
|
|
|
|
/// Assuming that the matrix is a proper rotation matrix, slerp performs
|
|
|
|
|
/// a spherical-linear interpolation with another rotation matrix.
|
|
|
|
|
/// </summary>
|
|
|
|
|
/// <param name="target">The destination basis for interpolation.</param>
|
|
|
|
|
/// <param name="weight">A value on the range of 0.0 to 1.0, representing the amount of interpolation.</param>
|
|
|
|
|
/// <returns>The resulting basis matrix of the interpolation.</returns>
|
|
|
|
|
public Basis Slerp(Basis target, real_t weight)
|
2020-01-23 15:54:34 +01:00
|
|
|
|
{
|
2020-07-07 23:29:35 +02:00
|
|
|
|
Quat from = new Quat(this);
|
|
|
|
|
Quat to = new Quat(target);
|
2020-01-23 15:54:34 +01:00
|
|
|
|
|
2020-07-07 23:29:35 +02:00
|
|
|
|
Basis b = new Basis(from.Slerp(to, weight));
|
|
|
|
|
b.Row0 *= Mathf.Lerp(Row0.Length(), target.Row0.Length(), weight);
|
|
|
|
|
b.Row1 *= Mathf.Lerp(Row1.Length(), target.Row1.Length(), weight);
|
|
|
|
|
b.Row2 *= Mathf.Lerp(Row2.Length(), target.Row2.Length(), weight);
|
2020-01-23 15:54:34 +01:00
|
|
|
|
|
|
|
|
|
return b;
|
|
|
|
|
}
|
|
|
|
|
|
2020-07-07 23:29:35 +02:00
|
|
|
|
/// <summary>
|
|
|
|
|
/// Transposed dot product with the X axis of the matrix.
|
|
|
|
|
/// </summary>
|
|
|
|
|
/// <param name="with">A vector to calculate the dot product with.</param>
|
|
|
|
|
/// <returns>The resulting dot product.</returns>
|
2018-03-01 08:51:35 +01:00
|
|
|
|
public real_t Tdotx(Vector3 with)
|
2017-10-02 23:24:00 +02:00
|
|
|
|
{
|
2019-02-23 18:22:27 +01:00
|
|
|
|
return this.Row0[0] * with[0] + this.Row1[0] * with[1] + this.Row2[0] * with[2];
|
2017-10-02 23:24:00 +02:00
|
|
|
|
}
|
|
|
|
|
|
2020-07-07 23:29:35 +02:00
|
|
|
|
/// <summary>
|
|
|
|
|
/// Transposed dot product with the Y axis of the matrix.
|
|
|
|
|
/// </summary>
|
|
|
|
|
/// <param name="with">A vector to calculate the dot product with.</param>
|
|
|
|
|
/// <returns>The resulting dot product.</returns>
|
2018-03-01 08:51:35 +01:00
|
|
|
|
public real_t Tdoty(Vector3 with)
|
2017-10-02 23:24:00 +02:00
|
|
|
|
{
|
2019-02-23 18:22:27 +01:00
|
|
|
|
return this.Row0[1] * with[0] + this.Row1[1] * with[1] + this.Row2[1] * with[2];
|
2017-10-02 23:24:00 +02:00
|
|
|
|
}
|
|
|
|
|
|
2020-07-07 23:29:35 +02:00
|
|
|
|
/// <summary>
|
|
|
|
|
/// Transposed dot product with the Z axis of the matrix.
|
|
|
|
|
/// </summary>
|
|
|
|
|
/// <param name="with">A vector to calculate the dot product with.</param>
|
|
|
|
|
/// <returns>The resulting dot product.</returns>
|
2018-03-01 08:51:35 +01:00
|
|
|
|
public real_t Tdotz(Vector3 with)
|
2017-10-02 23:24:00 +02:00
|
|
|
|
{
|
2019-02-23 18:22:27 +01:00
|
|
|
|
return this.Row0[2] * with[0] + this.Row1[2] * with[1] + this.Row2[2] * with[2];
|
2017-10-02 23:24:00 +02:00
|
|
|
|
}
|
|
|
|
|
|
2020-07-07 23:29:35 +02:00
|
|
|
|
/// <summary>
|
|
|
|
|
/// Returns the transposed version of the basis matrix.
|
|
|
|
|
/// </summary>
|
|
|
|
|
/// <returns>The transposed basis matrix.</returns>
|
2017-11-21 23:32:19 +01:00
|
|
|
|
public Basis Transposed()
|
2017-10-02 23:24:00 +02:00
|
|
|
|
{
|
2018-04-08 05:30:43 +02:00
|
|
|
|
var tr = this;
|
2017-10-02 23:24:00 +02:00
|
|
|
|
|
2019-02-23 18:22:27 +01:00
|
|
|
|
real_t temp = tr.Row0[1];
|
|
|
|
|
tr.Row0[1] = tr.Row1[0];
|
|
|
|
|
tr.Row1[0] = temp;
|
2017-10-02 23:24:00 +02:00
|
|
|
|
|
2019-02-23 18:22:27 +01:00
|
|
|
|
temp = tr.Row0[2];
|
|
|
|
|
tr.Row0[2] = tr.Row2[0];
|
|
|
|
|
tr.Row2[0] = temp;
|
2017-10-02 23:24:00 +02:00
|
|
|
|
|
2019-02-23 18:22:27 +01:00
|
|
|
|
temp = tr.Row1[2];
|
|
|
|
|
tr.Row1[2] = tr.Row2[1];
|
|
|
|
|
tr.Row2[1] = temp;
|
2017-10-02 23:24:00 +02:00
|
|
|
|
|
|
|
|
|
return tr;
|
|
|
|
|
}
|
|
|
|
|
|
2020-07-07 23:29:35 +02:00
|
|
|
|
/// <summary>
|
|
|
|
|
/// Returns a vector transformed (multiplied) by the basis matrix.
|
|
|
|
|
/// </summary>
|
|
|
|
|
/// <param name="v">A vector to transform.</param>
|
2020-07-22 22:46:04 +02:00
|
|
|
|
/// <returns>The transformed vector.</returns>
|
2017-11-21 23:32:19 +01:00
|
|
|
|
public Vector3 Xform(Vector3 v)
|
2017-10-02 23:24:00 +02:00
|
|
|
|
{
|
|
|
|
|
return new Vector3
|
|
|
|
|
(
|
2019-02-23 18:22:27 +01:00
|
|
|
|
this.Row0.Dot(v),
|
|
|
|
|
this.Row1.Dot(v),
|
|
|
|
|
this.Row2.Dot(v)
|
2017-10-02 23:24:00 +02:00
|
|
|
|
);
|
|
|
|
|
}
|
|
|
|
|
|
2020-07-07 23:29:35 +02:00
|
|
|
|
/// <summary>
|
|
|
|
|
/// Returns a vector transformed (multiplied) by the transposed basis matrix.
|
|
|
|
|
///
|
|
|
|
|
/// Note: This results in a multiplication by the inverse of the
|
|
|
|
|
/// basis matrix only if it represents a rotation-reflection.
|
|
|
|
|
/// </summary>
|
|
|
|
|
/// <param name="v">A vector to inversely transform.</param>
|
2020-07-22 22:46:04 +02:00
|
|
|
|
/// <returns>The inversely transformed vector.</returns>
|
2017-11-21 23:32:19 +01:00
|
|
|
|
public Vector3 XformInv(Vector3 v)
|
2017-10-02 23:24:00 +02:00
|
|
|
|
{
|
|
|
|
|
return new Vector3
|
|
|
|
|
(
|
2019-02-23 18:22:27 +01:00
|
|
|
|
this.Row0[0] * v.x + this.Row1[0] * v.y + this.Row2[0] * v.z,
|
|
|
|
|
this.Row0[1] * v.x + this.Row1[1] * v.y + this.Row2[1] * v.z,
|
|
|
|
|
this.Row0[2] * v.x + this.Row1[2] * v.y + this.Row2[2] * v.z
|
2017-10-02 23:24:00 +02:00
|
|
|
|
);
|
|
|
|
|
}
|
|
|
|
|
|
2020-07-07 23:29:35 +02:00
|
|
|
|
/// <summary>
|
|
|
|
|
/// Returns the basis's rotation in the form of a quaternion.
|
|
|
|
|
/// See <see cref="GetEuler()"/> if you need Euler angles, but keep in
|
|
|
|
|
/// mind that quaternions should generally be preferred to Euler angles.
|
|
|
|
|
/// </summary>
|
|
|
|
|
/// <returns>A <see cref="Godot.Quat"/> representing the basis's rotation.</returns>
|
2018-11-20 02:08:19 +01:00
|
|
|
|
public Quat Quat()
|
|
|
|
|
{
|
2019-02-23 18:22:27 +01:00
|
|
|
|
real_t trace = Row0[0] + Row1[1] + Row2[2];
|
2018-09-07 03:08:16 +02:00
|
|
|
|
|
2018-11-20 02:08:19 +01:00
|
|
|
|
if (trace > 0.0f)
|
|
|
|
|
{
|
2018-09-07 03:08:16 +02:00
|
|
|
|
real_t s = Mathf.Sqrt(trace + 1.0f) * 2f;
|
|
|
|
|
real_t inv_s = 1f / s;
|
|
|
|
|
return new Quat(
|
2019-02-23 18:22:27 +01:00
|
|
|
|
(Row2[1] - Row1[2]) * inv_s,
|
|
|
|
|
(Row0[2] - Row2[0]) * inv_s,
|
|
|
|
|
(Row1[0] - Row0[1]) * inv_s,
|
2018-09-07 03:08:16 +02:00
|
|
|
|
s * 0.25f
|
|
|
|
|
);
|
|
|
|
|
}
|
|
|
|
|
|
2019-02-23 18:22:27 +01:00
|
|
|
|
if (Row0[0] > Row1[1] && Row0[0] > Row2[2])
|
2018-11-20 02:08:19 +01:00
|
|
|
|
{
|
2019-02-23 18:22:27 +01:00
|
|
|
|
real_t s = Mathf.Sqrt(Row0[0] - Row1[1] - Row2[2] + 1.0f) * 2f;
|
2018-09-07 03:08:16 +02:00
|
|
|
|
real_t inv_s = 1f / s;
|
|
|
|
|
return new Quat(
|
|
|
|
|
s * 0.25f,
|
2019-02-23 18:22:27 +01:00
|
|
|
|
(Row0[1] + Row1[0]) * inv_s,
|
|
|
|
|
(Row0[2] + Row2[0]) * inv_s,
|
|
|
|
|
(Row2[1] - Row1[2]) * inv_s
|
2018-09-07 03:08:16 +02:00
|
|
|
|
);
|
|
|
|
|
}
|
|
|
|
|
|
2019-02-23 18:22:27 +01:00
|
|
|
|
if (Row1[1] > Row2[2])
|
2018-11-20 02:08:19 +01:00
|
|
|
|
{
|
2019-02-23 18:22:27 +01:00
|
|
|
|
real_t s = Mathf.Sqrt(-Row0[0] + Row1[1] - Row2[2] + 1.0f) * 2f;
|
2018-09-07 03:08:16 +02:00
|
|
|
|
real_t inv_s = 1f / s;
|
|
|
|
|
return new Quat(
|
2019-02-23 18:22:27 +01:00
|
|
|
|
(Row0[1] + Row1[0]) * inv_s,
|
2018-09-07 03:08:16 +02:00
|
|
|
|
s * 0.25f,
|
2019-02-23 18:22:27 +01:00
|
|
|
|
(Row1[2] + Row2[1]) * inv_s,
|
|
|
|
|
(Row0[2] - Row2[0]) * inv_s
|
2018-09-07 03:08:16 +02:00
|
|
|
|
);
|
2018-11-20 02:08:19 +01:00
|
|
|
|
}
|
|
|
|
|
else
|
|
|
|
|
{
|
2019-02-23 18:22:27 +01:00
|
|
|
|
real_t s = Mathf.Sqrt(-Row0[0] - Row1[1] + Row2[2] + 1.0f) * 2f;
|
2018-09-07 03:08:16 +02:00
|
|
|
|
real_t inv_s = 1f / s;
|
|
|
|
|
return new Quat(
|
2019-02-23 18:22:27 +01:00
|
|
|
|
(Row0[2] + Row2[0]) * inv_s,
|
|
|
|
|
(Row1[2] + Row2[1]) * inv_s,
|
2018-09-07 03:08:16 +02:00
|
|
|
|
s * 0.25f,
|
2019-02-23 18:22:27 +01:00
|
|
|
|
(Row1[0] - Row0[1]) * inv_s
|
2018-09-07 03:08:16 +02:00
|
|
|
|
);
|
|
|
|
|
}
|
|
|
|
|
}
|
2018-03-01 08:51:35 +01:00
|
|
|
|
|
2019-08-09 05:29:45 +02:00
|
|
|
|
private static readonly Basis[] _orthoBases = {
|
|
|
|
|
new Basis(1f, 0f, 0f, 0f, 1f, 0f, 0f, 0f, 1f),
|
|
|
|
|
new Basis(0f, -1f, 0f, 1f, 0f, 0f, 0f, 0f, 1f),
|
|
|
|
|
new Basis(-1f, 0f, 0f, 0f, -1f, 0f, 0f, 0f, 1f),
|
|
|
|
|
new Basis(0f, 1f, 0f, -1f, 0f, 0f, 0f, 0f, 1f),
|
|
|
|
|
new Basis(1f, 0f, 0f, 0f, 0f, -1f, 0f, 1f, 0f),
|
|
|
|
|
new Basis(0f, 0f, 1f, 1f, 0f, 0f, 0f, 1f, 0f),
|
|
|
|
|
new Basis(-1f, 0f, 0f, 0f, 0f, 1f, 0f, 1f, 0f),
|
|
|
|
|
new Basis(0f, 0f, -1f, -1f, 0f, 0f, 0f, 1f, 0f),
|
|
|
|
|
new Basis(1f, 0f, 0f, 0f, -1f, 0f, 0f, 0f, -1f),
|
|
|
|
|
new Basis(0f, 1f, 0f, 1f, 0f, 0f, 0f, 0f, -1f),
|
|
|
|
|
new Basis(-1f, 0f, 0f, 0f, 1f, 0f, 0f, 0f, -1f),
|
|
|
|
|
new Basis(0f, -1f, 0f, -1f, 0f, 0f, 0f, 0f, -1f),
|
|
|
|
|
new Basis(1f, 0f, 0f, 0f, 0f, 1f, 0f, -1f, 0f),
|
|
|
|
|
new Basis(0f, 0f, -1f, 1f, 0f, 0f, 0f, -1f, 0f),
|
|
|
|
|
new Basis(-1f, 0f, 0f, 0f, 0f, -1f, 0f, -1f, 0f),
|
|
|
|
|
new Basis(0f, 0f, 1f, -1f, 0f, 0f, 0f, -1f, 0f),
|
|
|
|
|
new Basis(0f, 0f, 1f, 0f, 1f, 0f, -1f, 0f, 0f),
|
|
|
|
|
new Basis(0f, -1f, 0f, 0f, 0f, 1f, -1f, 0f, 0f),
|
|
|
|
|
new Basis(0f, 0f, -1f, 0f, -1f, 0f, -1f, 0f, 0f),
|
|
|
|
|
new Basis(0f, 1f, 0f, 0f, 0f, -1f, -1f, 0f, 0f),
|
|
|
|
|
new Basis(0f, 0f, 1f, 0f, -1f, 0f, 1f, 0f, 0f),
|
|
|
|
|
new Basis(0f, 1f, 0f, 0f, 0f, 1f, 1f, 0f, 0f),
|
|
|
|
|
new Basis(0f, 0f, -1f, 0f, 1f, 0f, 1f, 0f, 0f),
|
|
|
|
|
new Basis(0f, -1f, 0f, 0f, 0f, -1f, 1f, 0f, 0f)
|
|
|
|
|
};
|
|
|
|
|
|
|
|
|
|
private static readonly Basis _identity = new Basis(1, 0, 0, 0, 1, 0, 0, 0, 1);
|
|
|
|
|
private static readonly Basis _flipX = new Basis(-1, 0, 0, 0, 1, 0, 0, 0, 1);
|
|
|
|
|
private static readonly Basis _flipY = new Basis(1, 0, 0, 0, -1, 0, 0, 0, 1);
|
|
|
|
|
private static readonly Basis _flipZ = new Basis(1, 0, 0, 0, 1, 0, 0, 0, -1);
|
|
|
|
|
|
2020-07-07 23:29:35 +02:00
|
|
|
|
/// <summary>
|
|
|
|
|
/// The identity basis, with no rotation or scaling applied.
|
|
|
|
|
/// This is used as a replacement for `Basis()` in GDScript.
|
|
|
|
|
/// Do not use `new Basis()` with no arguments in C#, because it sets all values to zero.
|
|
|
|
|
/// </summary>
|
|
|
|
|
/// <value>Equivalent to `new Basis(Vector3.Right, Vector3.Up, Vector3.Back)`.</value>
|
2019-08-09 05:29:45 +02:00
|
|
|
|
public static Basis Identity { get { return _identity; } }
|
2020-07-07 23:29:35 +02:00
|
|
|
|
/// <summary>
|
|
|
|
|
/// The basis that will flip something along the X axis when used in a transformation.
|
|
|
|
|
/// </summary>
|
|
|
|
|
/// <value>Equivalent to `new Basis(Vector3.Left, Vector3.Up, Vector3.Back)`.</value>
|
2019-08-09 05:29:45 +02:00
|
|
|
|
public static Basis FlipX { get { return _flipX; } }
|
2020-07-07 23:29:35 +02:00
|
|
|
|
/// <summary>
|
|
|
|
|
/// The basis that will flip something along the Y axis when used in a transformation.
|
|
|
|
|
/// </summary>
|
|
|
|
|
/// <value>Equivalent to `new Basis(Vector3.Right, Vector3.Down, Vector3.Back)`.</value>
|
2019-08-09 05:29:45 +02:00
|
|
|
|
public static Basis FlipY { get { return _flipY; } }
|
2020-07-07 23:29:35 +02:00
|
|
|
|
/// <summary>
|
|
|
|
|
/// The basis that will flip something along the Z axis when used in a transformation.
|
|
|
|
|
/// </summary>
|
|
|
|
|
/// <value>Equivalent to `new Basis(Vector3.Right, Vector3.Up, Vector3.Forward)`.</value>
|
2019-08-09 05:29:45 +02:00
|
|
|
|
public static Basis FlipZ { get { return _flipZ; } }
|
|
|
|
|
|
2020-07-07 23:29:35 +02:00
|
|
|
|
/// <summary>
|
|
|
|
|
/// Constructs a pure rotation basis matrix from the given quaternion.
|
|
|
|
|
/// </summary>
|
|
|
|
|
/// <param name="quat">The quaternion to create the basis from.</param>
|
2017-10-02 23:24:00 +02:00
|
|
|
|
public Basis(Quat quat)
|
|
|
|
|
{
|
2018-08-19 15:27:15 +02:00
|
|
|
|
real_t s = 2.0f / quat.LengthSquared;
|
2018-03-01 08:51:35 +01:00
|
|
|
|
|
|
|
|
|
real_t xs = quat.x * s;
|
|
|
|
|
real_t ys = quat.y * s;
|
|
|
|
|
real_t zs = quat.z * s;
|
|
|
|
|
real_t wx = quat.w * xs;
|
|
|
|
|
real_t wy = quat.w * ys;
|
|
|
|
|
real_t wz = quat.w * zs;
|
|
|
|
|
real_t xx = quat.x * xs;
|
|
|
|
|
real_t xy = quat.x * ys;
|
|
|
|
|
real_t xz = quat.x * zs;
|
|
|
|
|
real_t yy = quat.y * ys;
|
|
|
|
|
real_t yz = quat.y * zs;
|
|
|
|
|
real_t zz = quat.z * zs;
|
2017-10-02 23:24:00 +02:00
|
|
|
|
|
2019-02-23 18:22:27 +01:00
|
|
|
|
Row0 = new Vector3(1.0f - (yy + zz), xy - wz, xz + wy);
|
|
|
|
|
Row1 = new Vector3(xy + wz, 1.0f - (xx + zz), yz - wx);
|
|
|
|
|
Row2 = new Vector3(xz - wy, yz + wx, 1.0f - (xx + yy));
|
2017-10-02 23:24:00 +02:00
|
|
|
|
}
|
2018-05-17 15:31:14 +02:00
|
|
|
|
|
2020-07-07 23:29:35 +02:00
|
|
|
|
/// <summary>
|
|
|
|
|
/// Constructs a pure rotation basis matrix from the given Euler angles
|
|
|
|
|
/// (in the YXZ convention: when *composing*, first Y, then X, and Z last),
|
|
|
|
|
/// given in the vector format as (X angle, Y angle, Z angle).
|
|
|
|
|
///
|
|
|
|
|
/// Consider using the <see cref="Basis(Quat)"/> constructor instead, which
|
|
|
|
|
/// uses a <see cref="Godot.Quat"/> quaternion instead of Euler angles.
|
|
|
|
|
/// </summary>
|
|
|
|
|
/// <param name="eulerYXZ">The Euler angles to create the basis from.</param>
|
|
|
|
|
public Basis(Vector3 eulerYXZ)
|
2018-05-17 15:31:14 +02:00
|
|
|
|
{
|
|
|
|
|
real_t c;
|
|
|
|
|
real_t s;
|
|
|
|
|
|
2020-07-07 23:29:35 +02:00
|
|
|
|
c = Mathf.Cos(eulerYXZ.x);
|
|
|
|
|
s = Mathf.Sin(eulerYXZ.x);
|
2018-05-20 12:29:54 +02:00
|
|
|
|
var xmat = new Basis(1, 0, 0, 0, c, -s, 0, s, c);
|
2018-05-17 15:31:14 +02:00
|
|
|
|
|
2020-07-07 23:29:35 +02:00
|
|
|
|
c = Mathf.Cos(eulerYXZ.y);
|
|
|
|
|
s = Mathf.Sin(eulerYXZ.y);
|
2018-05-20 12:29:54 +02:00
|
|
|
|
var ymat = new Basis(c, 0, s, 0, 1, 0, -s, 0, c);
|
2018-05-17 15:31:14 +02:00
|
|
|
|
|
2020-07-07 23:29:35 +02:00
|
|
|
|
c = Mathf.Cos(eulerYXZ.z);
|
|
|
|
|
s = Mathf.Sin(eulerYXZ.z);
|
2018-05-20 12:29:54 +02:00
|
|
|
|
var zmat = new Basis(c, -s, 0, s, c, 0, 0, 0, 1);
|
2018-05-17 15:31:14 +02:00
|
|
|
|
|
|
|
|
|
this = ymat * xmat * zmat;
|
|
|
|
|
}
|
2017-10-02 23:24:00 +02:00
|
|
|
|
|
2020-07-07 23:29:35 +02:00
|
|
|
|
/// <summary>
|
|
|
|
|
/// Constructs a pure rotation basis matrix, rotated around the given `axis`
|
|
|
|
|
/// by `phi` (in radians). The axis must be a normalized vector.
|
|
|
|
|
/// </summary>
|
|
|
|
|
/// <param name="axis">The axis to rotate around. Must be normalized.</param>
|
|
|
|
|
/// <param name="phi">The angle to rotate, in radians.</param>
|
2018-03-01 08:51:35 +01:00
|
|
|
|
public Basis(Vector3 axis, real_t phi)
|
2017-10-02 23:24:00 +02:00
|
|
|
|
{
|
2019-05-17 11:11:12 +02:00
|
|
|
|
Vector3 axisSq = new Vector3(axis.x * axis.x, axis.y * axis.y, axis.z * axis.z);
|
2018-11-20 02:08:19 +01:00
|
|
|
|
real_t cosine = Mathf.Cos(phi);
|
2019-05-17 11:11:12 +02:00
|
|
|
|
Row0.x = axisSq.x + cosine * (1.0f - axisSq.x);
|
|
|
|
|
Row1.y = axisSq.y + cosine * (1.0f - axisSq.y);
|
|
|
|
|
Row2.z = axisSq.z + cosine * (1.0f - axisSq.z);
|
2017-10-02 23:24:00 +02:00
|
|
|
|
|
2019-05-17 11:11:12 +02:00
|
|
|
|
real_t sine = Mathf.Sin(phi);
|
|
|
|
|
real_t t = 1.0f - cosine;
|
|
|
|
|
|
|
|
|
|
real_t xyzt = axis.x * axis.y * t;
|
|
|
|
|
real_t zyxs = axis.z * sine;
|
|
|
|
|
Row0.y = xyzt - zyxs;
|
|
|
|
|
Row1.x = xyzt + zyxs;
|
|
|
|
|
|
|
|
|
|
xyzt = axis.x * axis.z * t;
|
|
|
|
|
zyxs = axis.y * sine;
|
|
|
|
|
Row0.z = xyzt + zyxs;
|
|
|
|
|
Row2.x = xyzt - zyxs;
|
|
|
|
|
|
|
|
|
|
xyzt = axis.y * axis.z * t;
|
|
|
|
|
zyxs = axis.x * sine;
|
|
|
|
|
Row1.z = xyzt - zyxs;
|
|
|
|
|
Row2.y = xyzt + zyxs;
|
2017-10-02 23:24:00 +02:00
|
|
|
|
}
|
|
|
|
|
|
2020-07-07 23:29:35 +02:00
|
|
|
|
/// <summary>
|
|
|
|
|
/// Constructs a basis matrix from 3 axis vectors (matrix columns).
|
|
|
|
|
/// </summary>
|
|
|
|
|
/// <param name="column0">The X vector, or Column0.</param>
|
|
|
|
|
/// <param name="column1">The Y vector, or Column1.</param>
|
|
|
|
|
/// <param name="column2">The Z vector, or Column2.</param>
|
2019-02-23 18:22:27 +01:00
|
|
|
|
public Basis(Vector3 column0, Vector3 column1, Vector3 column2)
|
2017-10-02 23:24:00 +02:00
|
|
|
|
{
|
2019-02-23 18:22:27 +01:00
|
|
|
|
Row0 = new Vector3(column0.x, column1.x, column2.x);
|
|
|
|
|
Row1 = new Vector3(column0.y, column1.y, column2.y);
|
|
|
|
|
Row2 = new Vector3(column0.z, column1.z, column2.z);
|
2018-11-20 22:23:09 +01:00
|
|
|
|
// Same as:
|
2019-02-23 18:22:27 +01:00
|
|
|
|
// Column0 = column0;
|
|
|
|
|
// Column1 = column1;
|
|
|
|
|
// Column2 = column2;
|
|
|
|
|
// We need to assign the struct fields here first so we can't do it that way...
|
2017-10-02 23:24:00 +02:00
|
|
|
|
}
|
|
|
|
|
|
2018-11-22 08:19:19 +01:00
|
|
|
|
// Arguments are named such that xy is equal to calling x.y
|
|
|
|
|
internal Basis(real_t xx, real_t yx, real_t zx, real_t xy, real_t yy, real_t zy, real_t xz, real_t yz, real_t zz)
|
2017-10-02 23:24:00 +02:00
|
|
|
|
{
|
2018-11-22 08:19:19 +01:00
|
|
|
|
Row0 = new Vector3(xx, yx, zx);
|
|
|
|
|
Row1 = new Vector3(xy, yy, zy);
|
|
|
|
|
Row2 = new Vector3(xz, yz, zz);
|
2017-10-02 23:24:00 +02:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
public static Basis operator *(Basis left, Basis right)
|
|
|
|
|
{
|
|
|
|
|
return new Basis
|
|
|
|
|
(
|
2019-02-23 18:22:27 +01:00
|
|
|
|
right.Tdotx(left.Row0), right.Tdoty(left.Row0), right.Tdotz(left.Row0),
|
|
|
|
|
right.Tdotx(left.Row1), right.Tdoty(left.Row1), right.Tdotz(left.Row1),
|
|
|
|
|
right.Tdotx(left.Row2), right.Tdoty(left.Row2), right.Tdotz(left.Row2)
|
2017-10-02 23:24:00 +02:00
|
|
|
|
);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
public static bool operator ==(Basis left, Basis right)
|
|
|
|
|
{
|
|
|
|
|
return left.Equals(right);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
public static bool operator !=(Basis left, Basis right)
|
|
|
|
|
{
|
|
|
|
|
return !left.Equals(right);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
public override bool Equals(object obj)
|
|
|
|
|
{
|
|
|
|
|
if (obj is Basis)
|
|
|
|
|
{
|
|
|
|
|
return Equals((Basis)obj);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
return false;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
public bool Equals(Basis other)
|
|
|
|
|
{
|
2019-02-23 18:22:27 +01:00
|
|
|
|
return Row0.Equals(other.Row0) && Row1.Equals(other.Row1) && Row2.Equals(other.Row2);
|
2017-10-02 23:24:00 +02:00
|
|
|
|
}
|
|
|
|
|
|
2020-07-07 23:29:35 +02:00
|
|
|
|
/// <summary>
|
|
|
|
|
/// Returns true if this basis and `other` are approximately equal, by running
|
|
|
|
|
/// <see cref="Vector3.IsEqualApprox(Vector3)"/> on each component.
|
|
|
|
|
/// </summary>
|
|
|
|
|
/// <param name="other">The other basis to compare.</param>
|
|
|
|
|
/// <returns>Whether or not the matrices are approximately equal.</returns>
|
2019-10-14 22:33:45 +02:00
|
|
|
|
public bool IsEqualApprox(Basis other)
|
|
|
|
|
{
|
|
|
|
|
return Row0.IsEqualApprox(other.Row0) && Row1.IsEqualApprox(other.Row1) && Row2.IsEqualApprox(other.Row2);
|
|
|
|
|
}
|
|
|
|
|
|
2017-10-02 23:24:00 +02:00
|
|
|
|
public override int GetHashCode()
|
|
|
|
|
{
|
2019-02-23 18:22:27 +01:00
|
|
|
|
return Row0.GetHashCode() ^ Row1.GetHashCode() ^ Row2.GetHashCode();
|
2017-10-02 23:24:00 +02:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
public override string ToString()
|
|
|
|
|
{
|
|
|
|
|
return String.Format("({0}, {1}, {2})", new object[]
|
|
|
|
|
{
|
2019-02-23 18:22:27 +01:00
|
|
|
|
Row0.ToString(),
|
|
|
|
|
Row1.ToString(),
|
|
|
|
|
Row2.ToString()
|
2017-10-02 23:24:00 +02:00
|
|
|
|
});
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
public string ToString(string format)
|
|
|
|
|
{
|
|
|
|
|
return String.Format("({0}, {1}, {2})", new object[]
|
|
|
|
|
{
|
2019-02-23 18:22:27 +01:00
|
|
|
|
Row0.ToString(format),
|
|
|
|
|
Row1.ToString(format),
|
|
|
|
|
Row2.ToString(format)
|
2017-10-02 23:24:00 +02:00
|
|
|
|
});
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
}
|