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 { 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) }); } } }