godot/modules/mono/glue/Managed/Files/Transform2D.cs

386 lines
10 KiB
C#
Raw Normal View History

2017-10-02 23:24:00 +02:00
using System;
using System.Runtime.InteropServices;
#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
{
2019-08-08 03:29:40 +02:00
[Serializable]
2017-10-02 23:24:00 +02:00
[StructLayout(LayoutKind.Sequential)]
public struct Transform2D : IEquatable<Transform2D>
{
public Vector2 x;
public Vector2 y;
2019-02-09 20:56:09 +01:00
public Vector2 origin;
2017-10-02 23:24:00 +02:00
public real_t Rotation
2017-10-02 23:24:00 +02:00
{
get
{
real_t det = BasisDeterminant();
Transform2D t = Orthonormalized();
if (det < 0)
{
t.ScaleBasis(new Vector2(1, -1));
}
return Mathf.Atan2(t.x.y, t.x.x);
}
set
{
Vector2 scale = Scale;
x.x = y.y = Mathf.Cos(value);
x.y = y.x = Mathf.Sin(value);
y.x *= -1;
Scale = scale;
}
2017-10-02 23:24:00 +02:00
}
public Vector2 Scale
{
get
{
real_t detSign = Mathf.Sign(BasisDeterminant());
return new Vector2(x.Length(), detSign * y.Length());
}
set
{
x = x.Normalized();
y = y.Normalized();
x *= value.x;
y *= value.y;
}
2017-10-02 23:24:00 +02:00
}
public Vector2 this[int rowIndex]
2017-10-02 23:24:00 +02:00
{
get
{
switch (rowIndex)
2017-10-02 23:24:00 +02:00
{
case 0:
return x;
case 1:
return y;
case 2:
2019-02-09 20:56:09 +01:00
return origin;
2017-10-02 23:24:00 +02:00
default:
throw new IndexOutOfRangeException();
}
}
set
{
switch (rowIndex)
2017-10-02 23:24:00 +02:00
{
case 0:
x = value;
return;
case 1:
y = value;
return;
case 2:
2019-02-09 20:56:09 +01:00
origin = value;
2017-10-02 23:24:00 +02:00
return;
default:
throw new IndexOutOfRangeException();
}
}
}
public real_t this[int rowIndex, int columnIndex]
2017-10-02 23:24:00 +02:00
{
get
{
switch (rowIndex)
2017-10-02 23:24:00 +02:00
{
case 0:
return x[columnIndex];
2017-10-02 23:24:00 +02:00
case 1:
return y[columnIndex];
2019-07-27 02:15:29 +02:00
case 2:
return origin[columnIndex];
2017-10-02 23:24:00 +02:00
default:
throw new IndexOutOfRangeException();
}
}
set
{
switch (rowIndex)
2017-10-02 23:24:00 +02:00
{
case 0:
x[columnIndex] = value;
2017-10-02 23:24:00 +02:00
return;
case 1:
y[columnIndex] = value;
2017-10-02 23:24:00 +02:00
return;
2019-07-27 02:15:29 +02:00
case 2:
origin[columnIndex] = value;
return;
2017-10-02 23:24:00 +02:00
default:
throw new IndexOutOfRangeException();
}
}
}
2017-11-21 23:32:19 +01:00
public Transform2D AffineInverse()
2017-10-02 23:24:00 +02:00
{
real_t det = BasisDeterminant();
2017-10-02 23:24:00 +02:00
if (det == 0)
throw new InvalidOperationException("Matrix determinant is zero and cannot be inverted.");
2017-10-02 23:24:00 +02:00
var inv = this;
2017-10-02 23:24:00 +02:00
real_t temp = inv[0, 0];
inv[0, 0] = inv[1, 1];
inv[1, 1] = temp;
2017-10-02 23:24:00 +02:00
real_t detInv = 1.0f / det;
inv[0] *= new Vector2(detInv, -detInv);
inv[1] *= new Vector2(-detInv, detInv);
2017-10-02 23:24:00 +02:00
2019-07-27 02:15:29 +02:00
inv[2] = inv.BasisXform(-inv[2]);
2017-10-02 23:24:00 +02:00
return inv;
}
private real_t BasisDeterminant()
{
return x.x * y.y - x.y * y.x;
}
2017-11-21 23:32:19 +01:00
public Vector2 BasisXform(Vector2 v)
2017-10-02 23:24:00 +02:00
{
2017-11-21 23:32:19 +01:00
return new Vector2(Tdotx(v), Tdoty(v));
2017-10-02 23:24:00 +02:00
}
2017-11-21 23:32:19 +01:00
public Vector2 BasisXformInv(Vector2 v)
2017-10-02 23:24:00 +02:00
{
2017-11-21 23:32:19 +01:00
return new Vector2(x.Dot(v), y.Dot(v));
2017-10-02 23:24:00 +02:00
}
public Transform2D InterpolateWith(Transform2D m, real_t c)
2017-10-02 23:24:00 +02:00
{
real_t r1 = Rotation;
real_t r2 = m.Rotation;
2017-10-02 23:24:00 +02:00
Vector2 s1 = Scale;
Vector2 s2 = m.Scale;
// Slerp rotation
2018-04-08 05:30:43 +02:00
var v1 = new Vector2(Mathf.Cos(r1), Mathf.Sin(r1));
var v2 = new Vector2(Mathf.Cos(r2), Mathf.Sin(r2));
2017-10-02 23:24:00 +02:00
real_t dot = v1.Dot(v2);
2017-10-02 23:24:00 +02:00
// Clamp dot to [-1, 1]
2018-04-08 05:39:35 +02:00
dot = dot < -1.0f ? -1.0f : (dot > 1.0f ? 1.0f : dot);
2017-10-02 23:24:00 +02:00
Vector2 v;
2017-10-02 23:24:00 +02:00
if (dot > 0.9995f)
{
// Linearly interpolate to avoid numerical precision issues
2017-11-21 23:32:19 +01:00
v = v1.LinearInterpolate(v2, c).Normalized();
2017-10-02 23:24:00 +02:00
}
else
{
real_t angle = c * Mathf.Acos(dot);
2017-11-21 23:32:19 +01:00
Vector2 v3 = (v2 - v1 * dot).Normalized();
v = v1 * Mathf.Cos(angle) + v3 * Mathf.Sin(angle);
2017-10-02 23:24:00 +02:00
}
// Extract parameters
2019-02-09 20:56:09 +01:00
Vector2 p1 = origin;
Vector2 p2 = m.origin;
2017-10-02 23:24:00 +02:00
// Construct matrix
2018-04-08 05:30:43 +02:00
var res = new Transform2D(Mathf.Atan2(v.y, v.x), p1.LinearInterpolate(p2, c));
Vector2 scale = s1.LinearInterpolate(s2, c);
2017-10-02 23:24:00 +02:00
res.x *= scale;
res.y *= scale;
return res;
}
2017-11-21 23:32:19 +01:00
public Transform2D Inverse()
2017-10-02 23:24:00 +02:00
{
2018-04-08 05:30:43 +02:00
var inv = this;
2017-10-02 23:24:00 +02:00
// Swap
real_t temp = inv.x.y;
2017-10-02 23:24:00 +02:00
inv.x.y = inv.y.x;
inv.y.x = temp;
2019-02-09 20:56:09 +01:00
inv.origin = inv.BasisXform(-inv.origin);
2017-10-02 23:24:00 +02:00
return inv;
}
2017-11-21 23:32:19 +01:00
public Transform2D Orthonormalized()
2017-10-02 23:24:00 +02:00
{
2018-04-08 05:30:43 +02:00
var on = this;
2017-10-02 23:24:00 +02:00
Vector2 onX = on.x;
Vector2 onY = on.y;
2017-11-21 23:32:19 +01:00
onX.Normalize();
2018-04-08 05:39:35 +02:00
onY = onY - onX * onX.Dot(onY);
2017-11-21 23:32:19 +01:00
onY.Normalize();
2017-10-02 23:24:00 +02:00
on.x = onX;
on.y = onY;
return on;
}
public Transform2D Rotated(real_t phi)
2017-10-02 23:24:00 +02:00
{
return this * new Transform2D(phi, new Vector2());
}
2017-11-21 23:32:19 +01:00
public Transform2D Scaled(Vector2 scale)
2017-10-02 23:24:00 +02:00
{
2018-04-08 05:30:43 +02:00
var copy = this;
2017-10-02 23:24:00 +02:00
copy.x *= scale;
copy.y *= scale;
2019-02-09 20:56:09 +01:00
copy.origin *= scale;
2017-10-02 23:24:00 +02:00
return copy;
}
private void ScaleBasis(Vector2 scale)
{
x.x *= scale.x;
x.y *= scale.y;
y.x *= scale.x;
y.y *= scale.y;
}
private real_t Tdotx(Vector2 with)
2017-10-02 23:24:00 +02:00
{
return this[0, 0] * with[0] + this[1, 0] * with[1];
}
private real_t Tdoty(Vector2 with)
2017-10-02 23:24:00 +02:00
{
return this[0, 1] * with[0] + this[1, 1] * with[1];
}
2017-11-21 23:32:19 +01:00
public Transform2D Translated(Vector2 offset)
2017-10-02 23:24:00 +02:00
{
2018-04-08 05:30:43 +02:00
var copy = this;
2019-02-09 20:56:09 +01:00
copy.origin += copy.BasisXform(offset);
2017-10-02 23:24:00 +02:00
return copy;
}
2017-11-21 23:32:19 +01:00
public Vector2 Xform(Vector2 v)
2017-10-02 23:24:00 +02:00
{
2019-02-09 20:56:09 +01:00
return new Vector2(Tdotx(v), Tdoty(v)) + origin;
2017-10-02 23:24:00 +02:00
}
2017-11-21 23:32:19 +01:00
public Vector2 XformInv(Vector2 v)
2017-10-02 23:24:00 +02:00
{
2019-02-09 20:56:09 +01:00
Vector2 vInv = v - origin;
2017-11-21 23:32:19 +01:00
return new Vector2(x.Dot(vInv), y.Dot(vInv));
2017-10-02 23:24:00 +02:00
}
// Constants
2019-02-09 20:56:09 +01:00
private static readonly Transform2D _identity = new Transform2D(1, 0, 0, 1, 0, 0);
private static readonly Transform2D _flipX = new Transform2D(-1, 0, 0, 1, 0, 0);
private static readonly Transform2D _flipY = new Transform2D(1, 0, 0, -1, 0, 0);
public static Transform2D Identity => _identity;
public static Transform2D FlipX => _flipX;
public static Transform2D FlipY => _flipY;
// Constructors
2019-02-09 20:56:09 +01:00
public Transform2D(Vector2 xAxis, Vector2 yAxis, Vector2 originPos)
2017-10-02 23:24:00 +02:00
{
x = xAxis;
y = yAxis;
2019-02-09 20:56:09 +01:00
origin = originPos;
2017-10-02 23:24:00 +02:00
}
// Arguments are named such that xy is equal to calling x.y
public Transform2D(real_t xx, real_t xy, real_t yx, real_t yy, real_t ox, real_t oy)
2017-10-02 23:24:00 +02:00
{
x = new Vector2(xx, xy);
y = new Vector2(yx, yy);
2019-02-09 20:56:09 +01:00
origin = new Vector2(ox, oy);
2017-10-02 23:24:00 +02:00
}
public Transform2D(real_t rot, Vector2 pos)
2017-10-02 23:24:00 +02:00
{
x.x = y.y = Mathf.Cos(rot);
x.y = y.x = Mathf.Sin(rot);
y.x *= -1;
2019-02-09 20:56:09 +01:00
origin = pos;
2017-10-02 23:24:00 +02:00
}
public static Transform2D operator *(Transform2D left, Transform2D right)
{
2019-02-09 20:56:09 +01:00
left.origin = left.Xform(right.origin);
2017-10-02 23:24:00 +02:00
real_t x0 = left.Tdotx(right.x);
real_t x1 = left.Tdoty(right.x);
real_t y0 = left.Tdotx(right.y);
real_t y1 = left.Tdoty(right.y);
2017-10-02 23:24:00 +02:00
left.x.x = x0;
left.x.y = x1;
left.y.x = y0;
left.y.y = y1;
return left;
}
public static bool operator ==(Transform2D left, Transform2D right)
{
return left.Equals(right);
}
public static bool operator !=(Transform2D left, Transform2D right)
{
return !left.Equals(right);
}
public override bool Equals(object obj)
{
return obj is Transform2D transform2D && Equals(transform2D);
2017-10-02 23:24:00 +02:00
}
public bool Equals(Transform2D other)
{
2019-02-09 20:56:09 +01:00
return x.Equals(other.x) && y.Equals(other.y) && origin.Equals(other.origin);
2017-10-02 23:24:00 +02:00
}
public override int GetHashCode()
{
2019-02-09 20:56:09 +01:00
return x.GetHashCode() ^ y.GetHashCode() ^ origin.GetHashCode();
2017-10-02 23:24:00 +02:00
}
public override string ToString()
{
return String.Format("({0}, {1}, {2})", new object[]
{
x.ToString(),
y.ToString(),
2019-02-09 20:56:09 +01:00
origin.ToString()
2017-10-02 23:24:00 +02:00
});
}
public string ToString(string format)
{
return String.Format("({0}, {1}, {2})", new object[]
{
x.ToString(format),
y.ToString(format),
2019-02-09 20:56:09 +01:00
origin.ToString(format)
2017-10-02 23:24:00 +02:00
});
}
}
}