asgardius/virtualx-engine
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions
virtualx-engine/modules/mono/glue/cs_files/Basis.cs
Aaron Franke ff97c97c93 Replace float with real_t, default Vectors, other misc C# improvements 
Replace float with real_t in most files, defined at the top of each file via using. Objects such as Vector3 now accept doubles as inputs, and convert to real_t internally. I've added default Vectors such as Vector3.Zero. Other misc C# improvements such as Mathf.RoundToInt(). Color continues to use float only because high precision is not needed for 8-bit color math and to keep things simple. Everything seems to compile and work fine, but testing is requested, as this is the first time I've ever contributed to Godot.
2018-03-22 21:53:21 -05:00

527 lines
16 KiB
C#
Raw Blame History

using System;
using System.Runtime.InteropServices;
#if REAL_T_IS_DOUBLE
using real_t = System.Double;
#else
using real_t = System.Single;
#endif
namespace Godot
{
[StructLayout(LayoutKind.Sequential)]
public struct Basis : IEquatable<Basis>
{
private static readonly Basis identity = new Basis
(
new Vector3(1f, 0f, 0f),
new Vector3(0f, 1f, 0f),
new Vector3(0f, 0f, 1f)
);
private static readonly Basis[] orthoBases = new Basis[24]
{
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)
};
public Vector3 x;
public Vector3 y;
public Vector3 z;
public static Basis Identity
{
get { return identity; }
}
public Vector3 Scale
{
get
{
return new Vector3
(
new Vector3(this[0, 0], this[1, 0], this[2, 0]).Length(),
new Vector3(this[0, 1], this[1, 1], this[2, 1]).Length(),
new Vector3(this[0, 2], this[1, 2], this[2, 2]).Length()
);
}
}
public Vector3 this[int index]
{
get
{
switch (index)
{
case 0:
return x;
case 1:
return y;
case 2:
return z;
default:
throw new IndexOutOfRangeException();
}
}
set
{
switch (index)
{
case 0:
x = value;
return;
case 1:
y = value;
return;
case 2:
z = value;
return;
default:
throw new IndexOutOfRangeException();
}
}
}
public real_t this[int index, int axis]
{
get
{
switch (index)
{
case 0:
return x[axis];
case 1:
return y[axis];
case 2:
return z[axis];
default:
throw new IndexOutOfRangeException();
}
}
set
{
switch (index)
{
case 0:
x[axis] = value;
return;
case 1:
y[axis] = value;
return;
case 2:
z[axis] = value;
return;
default:
throw new IndexOutOfRangeException();
}
}
}
internal static Basis CreateFromAxes(Vector3 xAxis, Vector3 yAxis, Vector3 zAxis)
{
return new Basis
(
new Vector3(xAxis.x, yAxis.x, zAxis.x),
new Vector3(xAxis.y, yAxis.y, zAxis.y),
new Vector3(xAxis.z, yAxis.z, zAxis.z)
);
}
public real_t Determinant()
{
return this[0, 0] * (this[1, 1] * this[2, 2] - this[2, 1] * this[1, 2]) -
this[1, 0] * (this[0, 1] * this[2, 2] - this[2, 1] * this[0, 2]) +
this[2, 0] * (this[0, 1] * this[1, 2] - this[1, 1] * this[0, 2]);
}
public Vector3 GetAxis(int axis)
{
return new Vector3(this[0, axis], this[1, axis], this[2, axis]);
}
public Vector3 GetEuler()
{
Basis m = this.Orthonormalized();
Vector3 euler;
euler.z = 0.0f;
real_t mxy = m.y[2];
if (mxy < 1.0f)
{
if (mxy > -1.0f)
{
euler.x = Mathf.Asin(-mxy);
euler.y = Mathf.Atan2(m.x[2], m.z[2]);
euler.z = Mathf.Atan2(m.y[0], m.y[1]);
}
else
{
euler.x = Mathf.PI * 0.5f;
euler.y = -Mathf.Atan2(-m.x[1], m.x[0]);
}
}
else
{
euler.x = -Mathf.PI * 0.5f;
euler.y = -Mathf.Atan2(m.x[1], m.x[0]);
}
return euler;
}
public int GetOrthogonalIndex()
{
Basis orth = this;
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
real_t v = orth[i, j];
if (v > 0.5f)
v = 1.0f;
else if (v < -0.5f)
v = -1.0f;
else
v = 0f;
orth[i, j] = v;
}
}
for (int i = 0; i < 24; i++)
{
if (orthoBases[i] == orth)
return i;
}
return 0;
}
public Basis Inverse()
{
Basis inv = this;
real_t[] co = new real_t[3]
{
inv[1, 1] * inv[2, 2] - inv[1, 2] * inv[2, 1],
inv[1, 2] * inv[2, 0] - inv[1, 0] * inv[2, 2],
inv[1, 0] * inv[2, 1] - inv[1, 1] * inv[2, 0]
};
real_t det = inv[0, 0] * co[0] + inv[0, 1] * co[1] + inv[0, 2] * co[2];
if (det == 0)
{
return new Basis
(
real_t.NaN, real_t.NaN, real_t.NaN,
real_t.NaN, real_t.NaN, real_t.NaN,
real_t.NaN, real_t.NaN, real_t.NaN
);
}
real_t s = 1.0f / det;
inv = new Basis
(
co[0] * s,
inv[0, 2] * inv[2, 1] - inv[0, 1] * inv[2, 2] * s,
inv[0, 1] * inv[1, 2] - inv[0, 2] * inv[1, 1] * s,
co[1] * s,
inv[0, 0] * inv[2, 2] - inv[0, 2] * inv[2, 0] * s,
inv[0, 2] * inv[1, 0] - inv[0, 0] * inv[1, 2] * s,
co[2] * s,
inv[0, 1] * inv[2, 0] - inv[0, 0] * inv[2, 1] * s,
inv[0, 0] * inv[1, 1] - inv[0, 1] * inv[1, 0] * s
);
return inv;
}
public Basis Orthonormalized()
{
Vector3 xAxis = GetAxis(0);
Vector3 yAxis = GetAxis(1);
Vector3 zAxis = GetAxis(2);
xAxis.Normalize();
yAxis = (yAxis - xAxis * (xAxis.Dot(yAxis)));
yAxis.Normalize();
zAxis = (zAxis - xAxis * (xAxis.Dot(zAxis)) - yAxis * (yAxis.Dot(zAxis)));
zAxis.Normalize();
return Basis.CreateFromAxes(xAxis, yAxis, zAxis);
}
public Basis Rotated(Vector3 axis, real_t phi)
{
return new Basis(axis, phi) * this;
}
public Basis Scaled(Vector3 scale)
{
Basis m = this;
m[0, 0] *= scale.x;
m[0, 1] *= scale.x;
m[0, 2] *= scale.x;
m[1, 0] *= scale.y;
m[1, 1] *= scale.y;
m[1, 2] *= scale.y;
m[2, 0] *= scale.z;
m[2, 1] *= scale.z;
m[2, 2] *= scale.z;
return m;
}
public real_t Tdotx(Vector3 with)
{
return this[0, 0] * with[0] + this[1, 0] * with[1] + this[2, 0] * with[2];
}
public real_t Tdoty(Vector3 with)
{
return this[0, 1] * with[0] + this[1, 1] * with[1] + this[2, 1] * with[2];
}
public real_t Tdotz(Vector3 with)
{
return this[0, 2] * with[0] + this[1, 2] * with[1] + this[2, 2] * with[2];
}
public Basis Transposed()
{
Basis tr = this;
real_t temp = this[0, 1];
this[0, 1] = this[1, 0];
this[1, 0] = temp;
temp = this[0, 2];
this[0, 2] = this[2, 0];
this[2, 0] = temp;
temp = this[1, 2];
this[1, 2] = this[2, 1];
this[2, 1] = temp;
return tr;
}
public Vector3 Xform(Vector3 v)
{
return new Vector3
(
this[0].Dot(v),
this[1].Dot(v),
this[2].Dot(v)
);
}
public Vector3 XformInv(Vector3 v)
{
return new Vector3
(
(this[0, 0] * v.x) + (this[1, 0] * v.y) + (this[2, 0] * v.z),
(this[0, 1] * v.x) + (this[1, 1] * v.y) + (this[2, 1] * v.z),
(this[0, 2] * v.x) + (this[1, 2] * v.y) + (this[2, 2] * v.z)
);
}
public Quat Quat() {
real_t trace = x[0] + y[1] + z[2];
if (trace > 0.0f) {
real_t s = Mathf.Sqrt(trace + 1.0f) * 2f;
real_t inv_s = 1f / s;
return new Quat(
(z[1] - y[2]) * inv_s,
(x[2] - z[0]) * inv_s,
(y[0] - x[1]) * inv_s,
s * 0.25f
);
} else if (x[0] > y[1] && x[0] > z[2]) {
real_t s = Mathf.Sqrt(x[0] - y[1] - z[2] + 1.0f) * 2f;
real_t inv_s = 1f / s;
return new Quat(
s * 0.25f,
(x[1] + y[0]) * inv_s,
(x[2] + z[0]) * inv_s,
(z[1] - y[2]) * inv_s
);
} else if (y[1] > z[2]) {
real_t s = Mathf.Sqrt(-x[0] + y[1] - z[2] + 1.0f) * 2f;
real_t inv_s = 1f / s;
return new Quat(
(x[1] + y[0]) * inv_s,
s * 0.25f,
(y[2] + z[1]) * inv_s,
(x[2] - z[0]) * inv_s
);
} else {
real_t s = Mathf.Sqrt(-x[0] - y[1] + z[2] + 1.0f) * 2f;
real_t inv_s = 1f / s;
return new Quat(
(x[2] + z[0]) * inv_s,
(y[2] + z[1]) * inv_s,
s * 0.25f,
(y[0] - x[1]) * inv_s
);
}
}
// Constructors
public Basis(Quat quat)
{
real_t s = 2.0f / quat.LengthSquared();
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;
this.x = new Vector3(1.0f - (yy + zz), xy - wz, xz + wy);
this.y = new Vector3(xy + wz, 1.0f - (xx + zz), yz - wx);
this.z = new Vector3(xz - wy, yz + wx, 1.0f - (xx + yy));
}
public Basis(Vector3 axis, real_t phi)
{
Vector3 axis_sq = new Vector3(axis.x * axis.x, axis.y * axis.y, axis.z * axis.z);
real_t cosine = Mathf.Cos( (real_t)phi);
real_t sine = Mathf.Sin( (real_t)phi);
this.x = new Vector3
(
axis_sq.x + cosine * (1.0f - axis_sq.x),
axis.x * axis.y * (1.0f - cosine) - axis.z * sine,
axis.z * axis.x * (1.0f - cosine) + axis.y * sine
);
this.y = new Vector3
(
axis.x * axis.y * (1.0f - cosine) + axis.z * sine,
axis_sq.y + cosine * (1.0f - axis_sq.y),
axis.y * axis.z * (1.0f - cosine) - axis.x * sine
);
this.z = new Vector3
(
axis.z * axis.x * (1.0f - cosine) - axis.y * sine,
axis.y * axis.z * (1.0f - cosine) + axis.x * sine,
axis_sq.z + cosine * (1.0f - axis_sq.z)
);
}
public Basis(Vector3 xAxis, Vector3 yAxis, Vector3 zAxis)
{
this.x = xAxis;
this.y = yAxis;
this.z = zAxis;
}
public Basis(real_t xx, real_t xy, real_t xz, real_t yx, real_t yy, real_t yz, real_t zx, real_t zy, real_t zz)
{
this.x = new Vector3(xx, yx, zx);
this.y = new Vector3(xy, yy, zy);
this.z = new Vector3(xz, yz, zz);
}
public static Basis operator *(Basis left, Basis right)
{
return new Basis
(
right.Tdotx(left[0]), right.Tdoty(left[0]), right.Tdotz(left[0]),
right.Tdotx(left[1]), right.Tdoty(left[1]), right.Tdotz(left[1]),
right.Tdotx(left[2]), right.Tdoty(left[2]), right.Tdotz(left[2])
);
}
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)
{
return x.Equals(other.x) && y.Equals(other.y) && z.Equals(other.z);
}
public override int GetHashCode()
{
return x.GetHashCode() ^ y.GetHashCode() ^ z.GetHashCode();
}
public override string ToString()
{
return String.Format("({0}, {1}, {2})", new object[]
{
this.x.ToString(),
this.y.ToString(),
this.z.ToString()
});
}
public string ToString(string format)
{
return String.Format("({0}, {1}, {2})", new object[]
{
this.x.ToString(format),
this.y.ToString(format),
this.z.ToString(format)
});
}
}
}
Reference in a new issue View git blame Copy permalink