├── .gitignore └── addons └── extra_math_cs ├── ExtraMath ├── Basis25D.cs ├── Double │ ├── AABBd.cs │ ├── Basis25Dd.cs │ ├── Basisd.cs │ ├── Mathd.cs │ ├── MathdEx.cs │ ├── Planed.cs │ ├── Quatd.cs │ ├── Rect2d.cs │ ├── Transform25Dd.cs │ ├── Transform2Dd.cs │ ├── Transformd.cs │ ├── Vector2d.cs │ ├── Vector3d.cs │ └── Vector4d.cs ├── Integer │ ├── AABBi.cs │ ├── Rect2i.cs │ ├── Vector2i.cs │ ├── Vector3i.cs │ └── Vector4i.cs ├── Transform25D.cs └── Vector4.cs ├── LICENSE.md ├── extramath.png ├── extramath.png.import └── plugin.cfg /.gitignore: -------------------------------------------------------------------------------- 1 | # Godot 4+ specific ignores 2 | .godot/ 3 | 4 | # Godot-specific ignores 5 | .import/ 6 | export.cfg 7 | export_presets.cfg 8 | 9 | # Imported translations (automatically generated from CSV files) 10 | *.translation 11 | 12 | # Mono-specific ignores 13 | .mono/ 14 | data_*/ 15 | mono_crash.*.json 16 | 17 | # System/tool-specific ignores 18 | .directory 19 | *~ 20 | 21 | # Unity 22 | *.meta 23 | -------------------------------------------------------------------------------- /addons/extra_math_cs/ExtraMath/Basis25D.cs: -------------------------------------------------------------------------------- 1 | #if GODOT 2 | using Godot; 3 | #elif UNITY_5_3_OR_NEWER 4 | using UnityEngine; 5 | #endif 6 | using System; 7 | using System.Runtime.InteropServices; 8 | 9 | #if GODOT_REAL_T_IS_DOUBLE 10 | using real_t = System.Double; 11 | #else 12 | using real_t = System.Single; 13 | #endif 14 | 15 | namespace ExtraMath 16 | { 17 | /// 18 | /// Basis25D structure for performing 2.5D transform math. 19 | /// Note: All code assumes that Y is UP in 3D, and DOWN in 2D. 20 | /// A top-down view has a Y axis component of (0, 0), with a Z axis component of (0, 1). 21 | /// For a front side view, Y is (0, -1) and Z is (0, 0). 22 | /// Remember that Godot's 2D mode has the Y axis pointing DOWN on the screen. 23 | /// 24 | [Serializable] 25 | [StructLayout(LayoutKind.Sequential)] 26 | public struct Basis25D : IEquatable 27 | { 28 | // Also matrix columns, the directions to move on screen for each unit change in 3D. 29 | public Vector2 x; 30 | public Vector2 y; 31 | public Vector2 z; 32 | 33 | // Also matrix rows, the parts of each vector that contribute to moving in a screen direction. 34 | // Setting a row to zero means no movement in that direction. 35 | public Vector3 Row0 36 | { 37 | get { return new Vector3(x.x, y.x, z.x); } 38 | set 39 | { 40 | x.x = value.x; 41 | y.x = value.y; 42 | z.x = value.z; 43 | } 44 | } 45 | 46 | public Vector3 Row1 47 | { 48 | get { return new Vector3(x.y, y.y, z.y); } 49 | set 50 | { 51 | x.y = value.x; 52 | y.y = value.y; 53 | z.y = value.z; 54 | } 55 | } 56 | 57 | public Vector2 this[int columnIndex] 58 | { 59 | get 60 | { 61 | switch (columnIndex) 62 | { 63 | case 0: return x; 64 | case 1: return y; 65 | case 2: return z; 66 | default: throw new IndexOutOfRangeException(); 67 | } 68 | } 69 | set 70 | { 71 | switch (columnIndex) 72 | { 73 | case 0: x = value; return; 74 | case 1: y = value; return; 75 | case 2: z = value; return; 76 | default: throw new IndexOutOfRangeException(); 77 | } 78 | } 79 | } 80 | 81 | public real_t this[int columnIndex, int rowIndex] 82 | { 83 | get 84 | { 85 | return this[columnIndex][rowIndex]; 86 | } 87 | set 88 | { 89 | Vector2 v = this[columnIndex]; 90 | v[rowIndex] = value; 91 | this[columnIndex] = v; 92 | } 93 | } 94 | 95 | #if GODOT 96 | private static readonly Basis25D _topDown = new Basis25D(1, 0, 0, 0, 0, 1); 97 | private static readonly Basis25D _frontSide = new Basis25D(1, 0, 0, -1, 0, 0); 98 | private static readonly Basis25D _fortyFive = new Basis25D(1, 0, 0, -0.70710678118f, 0, 0.70710678118f); 99 | private static readonly Basis25D _isometric = new Basis25D(0.86602540378f, 0.5f, 0, -1, -0.86602540378f, 0.5f); 100 | private static readonly Basis25D _obliqueY = new Basis25D(1, 0, -1, -1, 0, 1); 101 | private static readonly Basis25D _obliqueZ = new Basis25D(1, 0, 0, -1, -1, 1); 102 | #elif UNITY_5_3_OR_NEWER // Compared to Godot, flip x.y, y.y, and z.x. 103 | private static readonly Basis25D _topDown = new Basis25D(1, 0, 0, 0, 0, 1); 104 | private static readonly Basis25D _frontSide = new Basis25D(1, 0, 0, 1, 0, 0); 105 | private static readonly Basis25D _fortyFive = new Basis25D(1, 0, 0, 0.70710678118f, 0, 0.70710678118f); 106 | private static readonly Basis25D _isometric = new Basis25D(0.86602540378f, -0.5f, 0, 1, 0.86602540378f, 0.5f); 107 | private static readonly Basis25D _obliqueY = new Basis25D(1, 0, -1, 1, 0, 1); 108 | private static readonly Basis25D _obliqueZ = new Basis25D(1, 0, 0, 1, 1, 1); 109 | #endif 110 | 111 | public static Basis25D TopDown { get { return _topDown; } } 112 | public static Basis25D FrontSide { get { return _frontSide; } } 113 | public static Basis25D FortyFive { get { return _fortyFive; } } 114 | public static Basis25D Isometric { get { return _isometric; } } 115 | public static Basis25D ObliqueY { get { return _obliqueY; } } 116 | public static Basis25D ObliqueZ { get { return _obliqueZ; } } 117 | 118 | /// 119 | /// Creates a Dimetric Basis25D from the angle between the Y axis and the others. 120 | /// Dimetric(Tau/3) or Dimetric(2.09439510239) is the same as Isometric. 121 | /// Try to keep this number away from a multiple of Tau/4 (or Pi/2) radians. 122 | /// 123 | /// The angle, in radians, between the Y axis and the X/Z axes. 124 | public static Basis25D Dimetric(real_t angle) 125 | { 126 | real_t sin = Mathf.Sin(angle); 127 | real_t cos = Mathf.Cos(angle); 128 | #if GODOT 129 | return new Basis25D(sin, -cos, 0, -1, -sin, -cos); 130 | #elif UNITY_5_3_OR_NEWER 131 | return new Basis25D(sin, cos, 0, 1, sin, -cos); 132 | #endif 133 | } 134 | 135 | // Constructors 136 | public Basis25D(Basis25D b) 137 | { 138 | x = b.x; 139 | y = b.y; 140 | z = b.z; 141 | } 142 | public Basis25D(Vector2 xAxis, Vector2 yAxis, Vector2 zAxis) 143 | { 144 | x = xAxis; 145 | y = yAxis; 146 | z = zAxis; 147 | } 148 | public Basis25D(real_t xx, real_t xy, real_t yx, real_t yy, real_t zx, real_t zy) 149 | { 150 | x = new Vector2(xx, xy); 151 | y = new Vector2(yx, yy); 152 | z = new Vector2(zx, zy); 153 | } 154 | 155 | public static Basis25D operator *(Basis25D b, real_t s) 156 | { 157 | b.x *= s; 158 | b.y *= s; 159 | b.z *= s; 160 | return b; 161 | } 162 | 163 | public static Basis25D operator /(Basis25D b, real_t s) 164 | { 165 | b.x /= s; 166 | b.y /= s; 167 | b.z /= s; 168 | return b; 169 | } 170 | 171 | public static bool operator ==(Basis25D left, Basis25D right) 172 | { 173 | return left.Equals(right); 174 | } 175 | 176 | public static bool operator !=(Basis25D left, Basis25D right) 177 | { 178 | return !left.Equals(right); 179 | } 180 | 181 | public override bool Equals(object obj) 182 | { 183 | if (obj is Basis25D) 184 | { 185 | return Equals((Basis25D)obj); 186 | } 187 | return false; 188 | } 189 | 190 | public bool Equals(Basis25D other) 191 | { 192 | return x.Equals(other.x) && y.Equals(other.y) && z.Equals(other.z); 193 | } 194 | 195 | public override int GetHashCode() 196 | { 197 | return y.GetHashCode() ^ x.GetHashCode() ^ z.GetHashCode(); 198 | } 199 | 200 | public override string ToString() 201 | { 202 | string s = String.Format("({0}, {1}, {2})", new object[] 203 | { 204 | x.ToString(), 205 | y.ToString(), 206 | z.ToString() 207 | }); 208 | return s; 209 | } 210 | 211 | public string ToString(string format) 212 | { 213 | string s = String.Format("({0}, {1}, {2})", new object[] 214 | { 215 | x.ToString(format), 216 | y.ToString(format), 217 | z.ToString(format) 218 | }); 219 | return s; 220 | } 221 | } 222 | } 223 | -------------------------------------------------------------------------------- /addons/extra_math_cs/ExtraMath/Double/Basis25Dd.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | using System.Runtime.InteropServices; 3 | 4 | namespace ExtraMath 5 | { 6 | /// 7 | /// Basis25Dd structure for performing 2.5D transform math. 8 | /// Note: All code assumes that Y is UP in 3D, and DOWN in 2D. 9 | /// A top-down view has a Y axis component of (0, 0), with a Z axis component of (0, 1). 10 | /// For a front side view, Y is (0, -1) and Z is (0, 0). 11 | /// Remember that Godot's 2D mode has the Y axis pointing DOWN on the screen. 12 | /// 13 | [Serializable] 14 | [StructLayout(LayoutKind.Sequential)] 15 | public struct Basis25Dd : IEquatable 16 | { 17 | // Also matrix columns, the directions to move on screen for each unit change in 3D. 18 | public Vector2d x; 19 | public Vector2d y; 20 | public Vector2d z; 21 | 22 | // Also matrix rows, the parts of each vector that contribute to moving in a screen direction. 23 | // Setting a row to zero means no movement in that direction. 24 | public Vector3d Row0 25 | { 26 | get { return new Vector3d(x.x, y.x, z.x); } 27 | set 28 | { 29 | x.x = value.x; 30 | y.x = value.y; 31 | z.x = value.z; 32 | } 33 | } 34 | 35 | public Vector3d Row1 36 | { 37 | get { return new Vector3d(x.y, y.y, z.y); } 38 | set 39 | { 40 | x.y = value.x; 41 | y.y = value.y; 42 | z.y = value.z; 43 | } 44 | } 45 | 46 | public Vector2d this[int columnIndex] 47 | { 48 | get 49 | { 50 | switch (columnIndex) 51 | { 52 | case 0: return x; 53 | case 1: return y; 54 | case 2: return z; 55 | default: throw new IndexOutOfRangeException(); 56 | } 57 | } 58 | set 59 | { 60 | switch (columnIndex) 61 | { 62 | case 0: x = value; return; 63 | case 1: y = value; return; 64 | case 2: z = value; return; 65 | default: throw new IndexOutOfRangeException(); 66 | } 67 | } 68 | } 69 | 70 | public double this[int columnIndex, int rowIndex] 71 | { 72 | get 73 | { 74 | return this[columnIndex][rowIndex]; 75 | } 76 | set 77 | { 78 | Vector2d v = this[columnIndex]; 79 | v[rowIndex] = value; 80 | this[columnIndex] = v; 81 | } 82 | } 83 | 84 | private static readonly Basis25Dd _topDown = new Basis25Dd(1, 0, 0, 0, 0, 1); 85 | private static readonly Basis25Dd _frontSide = new Basis25Dd(1, 0, 0, -1, 0, 0); 86 | private static readonly Basis25Dd _fortyFive = new Basis25Dd(1, 0, 0, -0.70710678118, 0, 0.70710678118); 87 | private static readonly Basis25Dd _isometric = new Basis25Dd(0.86602540378, 0.5, 0, -1, -0.86602540378, 0.5); 88 | private static readonly Basis25Dd _obliqueY = new Basis25Dd(1, 0, -1, -1, 0, 1); 89 | private static readonly Basis25Dd _obliqueZ = new Basis25Dd(1, 0, 0, -1, -1, 1); 90 | 91 | public static Basis25Dd TopDown { get { return _topDown; } } 92 | public static Basis25Dd FrontSide { get { return _frontSide; } } 93 | public static Basis25Dd FortyFive { get { return _fortyFive; } } 94 | public static Basis25Dd Isometric { get { return _isometric; } } 95 | public static Basis25Dd ObliqueY { get { return _obliqueY; } } 96 | public static Basis25Dd ObliqueZ { get { return _obliqueZ; } } 97 | 98 | /// 99 | /// Creates a Dimetric Basis25Dd from the angle between the Y axis and the others. 100 | /// Dimetric(Tau/3) or Dimetric(2.09439510239) is the same as Isometric. 101 | /// Try to keep this number away from a multiple of Tau/4 (or Pi/2) radians. 102 | /// 103 | /// The angle, in radians, between the Y axis and the X/Z axes. 104 | public static Basis25Dd Dimetric(double angle) 105 | { 106 | double sin = Mathd.Sin(angle); 107 | double cos = Mathd.Cos(angle); 108 | return new Basis25Dd(sin, -cos, 0, -1, -sin, -cos); 109 | } 110 | 111 | public Basis25Dd(Basis25Dd b) 112 | { 113 | x = b.x; 114 | y = b.y; 115 | z = b.z; 116 | } 117 | public Basis25Dd(Vector2d xAxis, Vector2d yAxis, Vector2d zAxis) 118 | { 119 | x = xAxis; 120 | y = yAxis; 121 | z = zAxis; 122 | } 123 | public Basis25Dd(double xx, double xy, double yx, double yy, double zx, double zy) 124 | { 125 | x = new Vector2d(xx, xy); 126 | y = new Vector2d(yx, yy); 127 | z = new Vector2d(zx, zy); 128 | } 129 | 130 | public static explicit operator Basis25D(Basis25Dd value) 131 | { 132 | #if GODOT 133 | return new Basis25D((Godot.Vector2)value.x, (Godot.Vector2)value.y, (Godot.Vector2)value.z); 134 | #elif UNITY_5_3_OR_NEWER 135 | return new Basis25D((UnityEngine.Vector2)value.x, (UnityEngine.Vector2)value.y, (UnityEngine.Vector2)value.z); 136 | #endif 137 | } 138 | 139 | public static implicit operator Basis25Dd(Basis25D value) 140 | { 141 | return new Basis25Dd(value.x, value.y, value.z); 142 | } 143 | 144 | public static Basis25Dd operator *(Basis25Dd b, double s) 145 | { 146 | b.x *= s; 147 | b.y *= s; 148 | b.z *= s; 149 | return b; 150 | } 151 | 152 | public static Basis25Dd operator /(Basis25Dd b, double s) 153 | { 154 | b.x /= s; 155 | b.y /= s; 156 | b.z /= s; 157 | return b; 158 | } 159 | 160 | public static bool operator ==(Basis25Dd left, Basis25Dd right) 161 | { 162 | return left.Equals(right); 163 | } 164 | 165 | public static bool operator !=(Basis25Dd left, Basis25Dd right) 166 | { 167 | return !left.Equals(right); 168 | } 169 | 170 | public override bool Equals(object obj) 171 | { 172 | if (obj is Basis25Dd) 173 | { 174 | return Equals((Basis25Dd)obj); 175 | } 176 | return false; 177 | } 178 | 179 | public bool Equals(Basis25Dd other) 180 | { 181 | return x.Equals(other.x) && y.Equals(other.y) && z.Equals(other.z); 182 | } 183 | 184 | public override int GetHashCode() 185 | { 186 | return y.GetHashCode() ^ x.GetHashCode() ^ z.GetHashCode(); 187 | } 188 | 189 | public override string ToString() 190 | { 191 | string s = String.Format("({0}, {1}, {2})", new object[] 192 | { 193 | x.ToString(), 194 | y.ToString(), 195 | z.ToString() 196 | }); 197 | return s; 198 | } 199 | 200 | public string ToString(string format) 201 | { 202 | string s = String.Format("({0}, {1}, {2})", new object[] 203 | { 204 | x.ToString(format), 205 | y.ToString(format), 206 | z.ToString(format) 207 | }); 208 | return s; 209 | } 210 | } 211 | } 212 | -------------------------------------------------------------------------------- /addons/extra_math_cs/ExtraMath/Double/MathdEx.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | 3 | namespace ExtraMath 4 | { 5 | public static partial class Mathd 6 | { 7 | // Define constants with Decimal precision and cast down to double. 8 | 9 | /// 10 | /// The natural number `e`. 11 | /// 12 | public const double E = (double) 2.7182818284590452353602874714M; // 2.718281828459045 13 | 14 | /// 15 | /// The square root of 2. 16 | /// 17 | public const double Sqrt2 = (double) 1.4142135623730950488016887242M; // 1.414213562373095 18 | 19 | // If Godot is using single-precision, then we should be lenient with Epsilon comparisons. 20 | // Any Godot types imported into here would still be limited by the precision of real_t. 21 | /// 22 | /// A very small number used for float comparison with error tolerance. 23 | /// 1e-06 with single-precision floats, but 1e-14 if `REAL_T_IS_DOUBLE`. 24 | /// 25 | #if GODOT_REAL_T_IS_DOUBLE 26 | public const double Epsilon = 1e-14; 27 | #else 28 | public const double Epsilon = 1e-06f; 29 | #endif 30 | 31 | public static double Acosh(double x) 32 | { 33 | #if DEBUG 34 | if (x < 1.0) throw new ArgumentOutOfRangeException("Acosh failure: " + x + " is less than one."); 35 | #endif 36 | return Math.Log(x + Math.Sqrt(x * x - 1.0)); 37 | } 38 | 39 | /// 40 | /// Returns the amount of digits after the decimal place. 41 | /// 42 | /// The input value. 43 | /// The amount of digits. 44 | public static int DecimalCount(double s) 45 | { 46 | return DecimalCount((decimal)s); 47 | } 48 | 49 | /// 50 | /// Returns the amount of digits after the decimal place. 51 | /// 52 | /// The input value. 53 | /// The amount of digits. 54 | public static int DecimalCount(decimal s) 55 | { 56 | return BitConverter.GetBytes(decimal.GetBits(s)[3])[2]; 57 | } 58 | 59 | /// 60 | /// Rounds `s` upward (towards positive infinity). 61 | /// 62 | /// This is the same as , but returns an `int`. 63 | /// 64 | /// The number to ceil. 65 | /// The smallest whole number that is not less than `s`. 66 | public static int CeilToInt(double s) 67 | { 68 | return (int)Math.Ceiling(s); 69 | } 70 | 71 | /// 72 | /// Rounds `s` downward (towards negative infinity). 73 | /// 74 | /// This is the same as , but returns an `int`. 75 | /// 76 | /// The number to floor. 77 | /// The largest whole number that is not more than `s`. 78 | public static int FloorToInt(double s) 79 | { 80 | return (int)Math.Floor(s); 81 | } 82 | 83 | /// 84 | /// 85 | /// 86 | /// 87 | /// 88 | public static int RoundToInt(double s) 89 | { 90 | return (int)Math.Round(s); 91 | } 92 | 93 | /// 94 | /// Returns true if `a` and `b` are approximately equal to each other. 95 | /// The comparison is done using the provided tolerance value. 96 | /// If you want the tolerance to be calculated for you, use . 97 | /// 98 | /// One of the values. 99 | /// The other value. 100 | /// The pre-calculated tolerance value. 101 | /// A bool for whether or not the two values are equal. 102 | public static bool IsEqualApprox(double a, double b, double tolerance) 103 | { 104 | // Check for exact equality first, required to handle "infinity" values. 105 | if (a == b) 106 | { 107 | return true; 108 | } 109 | // Then check for approximate equality. 110 | return Abs(a - b) < tolerance; 111 | } 112 | } 113 | } 114 | -------------------------------------------------------------------------------- /addons/extra_math_cs/ExtraMath/Double/Planed.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | using System.Runtime.InteropServices; 3 | 4 | #if GODOT_REAL_T_IS_DOUBLE 5 | using real_t = System.Double; 6 | #else 7 | using real_t = System.Single; 8 | #endif 9 | 10 | namespace ExtraMath 11 | { 12 | /// 13 | /// Planed represents a normalized plane equation. 14 | /// "Over" or "Above" the plane is considered the side of 15 | /// the plane towards where the normal is pointing. 16 | /// 17 | [Serializable] 18 | [StructLayout(LayoutKind.Sequential)] 19 | public struct Planed : IEquatable 20 | { 21 | private Vector3d _normal; 22 | 23 | /// 24 | /// The normal of the plane, which must be normalized. 25 | /// In the scalar equation of the plane `ax + by + cz = d`, this is 26 | /// the vector `(a, b, c)`, where `d` is the property. 27 | /// 28 | /// Equivalent to `x`, `y`, and `z`. 29 | public Vector3d Normal 30 | { 31 | get { return _normal; } 32 | set { _normal = value; } 33 | } 34 | 35 | /// 36 | /// The X component of the plane's normal vector. 37 | /// 38 | /// Equivalent to 's X value. 39 | public double x 40 | { 41 | get 42 | { 43 | return _normal.x; 44 | } 45 | set 46 | { 47 | _normal.x = value; 48 | } 49 | } 50 | 51 | /// 52 | /// The Y component of the plane's normal vector. 53 | /// 54 | /// Equivalent to 's Y value. 55 | public double y 56 | { 57 | get 58 | { 59 | return _normal.y; 60 | } 61 | set 62 | { 63 | _normal.y = value; 64 | } 65 | } 66 | 67 | /// 68 | /// The Z component of the plane's normal vector. 69 | /// 70 | /// Equivalent to 's Z value. 71 | public double z 72 | { 73 | get 74 | { 75 | return _normal.z; 76 | } 77 | set 78 | { 79 | _normal.z = value; 80 | } 81 | } 82 | 83 | /// 84 | /// The distance from the origin to the plane (in the direction of 85 | /// ). This value is typically non-negative. 86 | /// In the scalar equation of the plane `ax + by + cz = d`, 87 | /// this is `d`, while the `(a, b, c)` coordinates are represented 88 | /// by the property. 89 | /// 90 | /// The plane's distance from the origin. 91 | public double D { get; set; } 92 | 93 | /// 94 | /// The center of the plane, the point where the normal line intersects the plane. 95 | /// 96 | /// Equivalent to multiplied by `D`. 97 | public Vector3d Center 98 | { 99 | get 100 | { 101 | return _normal * D; 102 | } 103 | set 104 | { 105 | _normal = value.Normalized(); 106 | D = value.Length(); 107 | } 108 | } 109 | 110 | /// 111 | /// Returns the shortest distance from this plane to the position `point`. 112 | /// 113 | /// The position to use for the calculation. 114 | /// The shortest distance. 115 | public double DistanceTo(Vector3d point) 116 | { 117 | return _normal.Dot(point) - D; 118 | } 119 | 120 | /// 121 | /// Returns true if point is inside the plane. 122 | /// Comparison uses a custom minimum epsilon threshold. 123 | /// 124 | /// The point to check. 125 | /// The tolerance threshold. 126 | /// A bool for whether or not the plane has the point. 127 | public bool HasPoint(Vector3d point, double epsilon = Mathd.Epsilon) 128 | { 129 | double dist = _normal.Dot(point) - D; 130 | return Mathd.Abs(dist) <= epsilon; 131 | } 132 | 133 | /// 134 | /// Returns the intersection point of the three planes: `b`, `c`, 135 | /// and this plane. If no intersection is found, `null` is returned. 136 | /// 137 | /// One of the three planes to use in the calculation. 138 | /// One of the three planes to use in the calculation. 139 | /// The intersection, or `null` if none is found. 140 | public Vector3d? Intersect3(Planed b, Planed c) 141 | { 142 | double denom = _normal.Cross(b._normal).Dot(c._normal); 143 | 144 | if (Mathd.IsZeroApprox(denom)) 145 | { 146 | return null; 147 | } 148 | 149 | Vector3d result = b._normal.Cross(c._normal) * D + 150 | c._normal.Cross(_normal) * b.D + 151 | _normal.Cross(b._normal) * c.D; 152 | 153 | return result / denom; 154 | } 155 | 156 | /// 157 | /// Returns the intersection point of a ray consisting of the 158 | /// position `from` and the direction normal `dir` with this plane. 159 | /// If no intersection is found, `null` is returned. 160 | /// 161 | /// The start of the ray. 162 | /// The direction of the ray, normalized. 163 | /// The intersection, or `null` if none is found. 164 | public Vector3d? IntersectRay(Vector3d from, Vector3d dir) 165 | { 166 | double den = _normal.Dot(dir); 167 | 168 | if (Mathd.IsZeroApprox(den)) 169 | { 170 | return null; 171 | } 172 | 173 | double dist = (_normal.Dot(from) - D) / den; 174 | 175 | // This is a ray, before the emitting pos (from) does not exist 176 | if (dist > Mathd.Epsilon) 177 | { 178 | return null; 179 | } 180 | 181 | return from + dir * -dist; 182 | } 183 | 184 | /// 185 | /// Returns the intersection point of a line segment from 186 | /// position `begin` to position `end` with this plane. 187 | /// If no intersection is found, `null` is returned. 188 | /// 189 | /// The start of the line segment. 190 | /// The end of the line segment. 191 | /// The intersection, or `null` if none is found. 192 | public Vector3d? IntersectSegment(Vector3d begin, Vector3d end) 193 | { 194 | Vector3d segment = begin - end; 195 | double den = _normal.Dot(segment); 196 | 197 | if (Mathd.IsZeroApprox(den)) 198 | { 199 | return null; 200 | } 201 | 202 | double dist = (_normal.Dot(begin) - D) / den; 203 | 204 | // Only allow dist to be in the range of 0 to 1, with tolerance. 205 | if (dist < -Mathd.Epsilon || dist > 1.0f + Mathd.Epsilon) 206 | { 207 | return null; 208 | } 209 | 210 | return begin + segment * -dist; 211 | } 212 | 213 | /// 214 | /// Returns true if `point` is located above the plane. 215 | /// 216 | /// The point to check. 217 | /// A bool for whether or not the point is above the plane. 218 | public bool IsPointOver(Vector3d point) 219 | { 220 | return _normal.Dot(point) > D; 221 | } 222 | 223 | /// 224 | /// Returns the plane scaled to unit length. 225 | /// 226 | /// A normalized version of the plane. 227 | public Planed Normalized() 228 | { 229 | double len = _normal.Length(); 230 | 231 | if (len == 0) 232 | { 233 | return new Planed(0, 0, 0, 0); 234 | } 235 | 236 | return new Planed(_normal / len, D / len); 237 | } 238 | 239 | /// 240 | /// Returns the orthogonal projection of `point` into the plane. 241 | /// 242 | /// The point to project. 243 | /// The projected point. 244 | public Vector3d Project(Vector3d point) 245 | { 246 | return point - _normal * DistanceTo(point); 247 | } 248 | 249 | // Constants 250 | private static readonly Planed _planeYZ = new Planed(1, 0, 0, 0); 251 | private static readonly Planed _planeXZ = new Planed(0, 1, 0, 0); 252 | private static readonly Planed _planeXY = new Planed(0, 0, 1, 0); 253 | 254 | /// 255 | /// A plane that extends in the Y and Z axes (normal vector points +X). 256 | /// 257 | /// Equivalent to `new Planed(1, 0, 0, 0)`. 258 | public static Planed PlaneYZ { get { return _planeYZ; } } 259 | 260 | /// 261 | /// A plane that extends in the X and Z axes (normal vector points +Y). 262 | /// 263 | /// Equivalent to `new Planed(0, 1, 0, 0)`. 264 | public static Planed PlaneXZ { get { return _planeXZ; } } 265 | 266 | /// 267 | /// A plane that extends in the X and Y axes (normal vector points +Z). 268 | /// 269 | /// Equivalent to `new Planed(0, 0, 1, 0)`. 270 | public static Planed PlaneXY { get { return _planeXY; } } 271 | 272 | /// 273 | /// Constructs a plane from four values. `a`, `b` and `c` become the 274 | /// components of the resulting plane's vector. 275 | /// `d` becomes the plane's distance from the origin. 276 | /// 277 | /// The X component of the plane's normal vector. 278 | /// The Y component of the plane's normal vector. 279 | /// The Z component of the plane's normal vector. 280 | /// The plane's distance from the origin. This value is typically non-negative. 281 | public Planed(double a, double b, double c, double d) 282 | { 283 | _normal = new Vector3d(a, b, c); 284 | this.D = d; 285 | } 286 | 287 | /// 288 | /// Constructs a plane from a normal vector and the plane's distance to the origin. 289 | /// 290 | /// The normal of the plane, must be normalized. 291 | /// The plane's distance from the origin. This value is typically non-negative. 292 | public Planed(Vector3d normal, double d) 293 | { 294 | this._normal = normal; 295 | this.D = d; 296 | } 297 | 298 | /// 299 | /// Constructs a plane from the three points, given in clockwise order. 300 | /// 301 | /// The first point. 302 | /// The second point. 303 | /// The third point. 304 | public Planed(Vector3d v1, Vector3d v2, Vector3d v3) 305 | { 306 | _normal = (v1 - v3).Cross(v1 - v2); 307 | _normal.Normalize(); 308 | D = _normal.Dot(v1); 309 | } 310 | 311 | #if GODOT 312 | public static explicit operator Godot.Plane(Planed value) 313 | { 314 | return new Godot.Plane((Godot.Vector3)value.Normal, (real_t)value.D); 315 | } 316 | 317 | public static implicit operator Planed(Godot.Plane value) 318 | { 319 | return new Planed(value.Normal, value.D); 320 | } 321 | #elif UNITY_5_3_OR_NEWER 322 | public static explicit operator UnityEngine.Plane(Planed value) 323 | { 324 | return new UnityEngine.Plane((UnityEngine.Vector3)value.Normal, (real_t)value.D); 325 | } 326 | 327 | public static implicit operator Planed(UnityEngine.Plane value) 328 | { 329 | return new Planed(value.normal, value.distance); 330 | } 331 | #endif 332 | 333 | public static Planed operator -(Planed plane) 334 | { 335 | return new Planed(-plane._normal, -plane.D); 336 | } 337 | 338 | public static bool operator ==(Planed left, Planed right) 339 | { 340 | return left.Equals(right); 341 | } 342 | 343 | public static bool operator !=(Planed left, Planed right) 344 | { 345 | return !left.Equals(right); 346 | } 347 | 348 | public override bool Equals(object obj) 349 | { 350 | if (obj is Planed) 351 | { 352 | return Equals((Planed)obj); 353 | } 354 | 355 | return false; 356 | } 357 | 358 | public bool Equals(Planed other) 359 | { 360 | return _normal == other._normal && D == other.D; 361 | } 362 | 363 | /// 364 | /// Returns true if this plane and `other` are approximately equal, by running 365 | /// on each component. 366 | /// 367 | /// The other plane to compare. 368 | /// Whether or not the planes are approximately equal. 369 | public bool IsEqualApprox(Planed other) 370 | { 371 | return _normal.IsEqualApprox(other._normal) && Mathd.IsEqualApprox(D, other.D); 372 | } 373 | 374 | public override int GetHashCode() 375 | { 376 | return _normal.GetHashCode() ^ D.GetHashCode(); 377 | } 378 | 379 | public override string ToString() 380 | { 381 | return String.Format("({0}, {1})", new object[] 382 | { 383 | _normal.ToString(), 384 | D.ToString() 385 | }); 386 | } 387 | 388 | public string ToString(string format) 389 | { 390 | return String.Format("({0}, {1})", new object[] 391 | { 392 | _normal.ToString(format), 393 | D.ToString(format) 394 | }); 395 | } 396 | } 397 | } 398 | -------------------------------------------------------------------------------- /addons/extra_math_cs/ExtraMath/Double/Quatd.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | using System.Runtime.InteropServices; 3 | 4 | #if GODOT_REAL_T_IS_DOUBLE 5 | using real_t = System.Double; 6 | #else 7 | using real_t = System.Single; 8 | #endif 9 | 10 | namespace ExtraMath 11 | { 12 | /// 13 | /// A unit quaternion used for representing 3D rotations. 14 | /// Quaternions need to be normalized to be used for rotation. 15 | /// 16 | /// It is similar to Basisd, which implements matrix representation of 17 | /// rotations, and can be parametrized using both an axis-angle pair 18 | /// or Euler angles. Basisd stores rotation, scale, and shearing, 19 | /// while Quatd only stores rotation. 20 | /// 21 | /// Due to its compactness and the way it is stored in memory, certain 22 | /// operations (obtaining axis-angle and performing SLERP, in particular) 23 | /// are more efficient and robust against floating-point errors. 24 | /// 25 | [Serializable] 26 | [StructLayout(LayoutKind.Sequential)] 27 | public struct Quatd : IEquatable 28 | { 29 | /// 30 | /// X component of the quaternion (imaginary `i` axis part). 31 | /// Quaternion components should usually not be manipulated directly. 32 | /// 33 | public double x; 34 | 35 | /// 36 | /// Y component of the quaternion (imaginary `j` axis part). 37 | /// Quaternion components should usually not be manipulated directly. 38 | /// 39 | public double y; 40 | 41 | /// 42 | /// Z component of the quaternion (imaginary `k` axis part). 43 | /// Quaternion components should usually not be manipulated directly. 44 | /// 45 | public double z; 46 | 47 | /// 48 | /// W component of the quaternion (real part). 49 | /// Quaternion components should usually not be manipulated directly. 50 | /// 51 | public double w; 52 | 53 | /// 54 | /// Access quaternion components using their index. 55 | /// 56 | /// `[0]` is equivalent to `.x`, `[1]` is equivalent to `.y`, `[2]` is equivalent to `.z`, `[3]` is equivalent to `.w`. 57 | public double this[int index] 58 | { 59 | get 60 | { 61 | switch (index) 62 | { 63 | case 0: 64 | return x; 65 | case 1: 66 | return y; 67 | case 2: 68 | return z; 69 | case 3: 70 | return w; 71 | default: 72 | throw new IndexOutOfRangeException(); 73 | } 74 | } 75 | set 76 | { 77 | switch (index) 78 | { 79 | case 0: 80 | x = value; 81 | break; 82 | case 1: 83 | y = value; 84 | break; 85 | case 2: 86 | z = value; 87 | break; 88 | case 3: 89 | w = value; 90 | break; 91 | default: 92 | throw new IndexOutOfRangeException(); 93 | } 94 | } 95 | } 96 | 97 | /// 98 | /// Returns the length (magnitude) of the quaternion. 99 | /// 100 | /// Equivalent to `Mathd.Sqrt(LengthSquared)`. 101 | public double Length 102 | { 103 | get { return Mathd.Sqrt(LengthSquared); } 104 | } 105 | 106 | /// 107 | /// Returns the squared length (squared magnitude) of the quaternion. 108 | /// This method runs faster than , so prefer it if 109 | /// you need to compare quaternions or need the squared length for some formula. 110 | /// 111 | /// Equivalent to `Dot(this)`. 112 | public double LengthSquared 113 | { 114 | get { return Dot(this); } 115 | } 116 | 117 | /// 118 | /// Performs a cubic spherical interpolation between quaternions `preA`, 119 | /// this vector, `b`, and `postB`, by the given amount `t`. 120 | /// 121 | /// The destination quaternion. 122 | /// A quaternion before this quaternion. 123 | /// A quaternion after `b`. 124 | /// A value on the range of 0.0 to 1.0, representing the amount of interpolation. 125 | /// The interpolated quaternion. 126 | public Quatd CubicSlerp(Quatd b, Quatd preA, Quatd postB, double weight) 127 | { 128 | double t2 = (1.0f - weight) * weight * 2f; 129 | Quatd sp = Slerp(b, weight); 130 | Quatd sq = preA.Slerpni(postB, weight); 131 | return sp.Slerpni(sq, t2); 132 | } 133 | 134 | /// 135 | /// Returns the dot product of two quaternions. 136 | /// 137 | /// The other quaternion. 138 | /// The dot product. 139 | public double Dot(Quatd b) 140 | { 141 | return x * b.x + y * b.y + z * b.z + w * b.w; 142 | } 143 | 144 | /// 145 | /// Returns Euler angles (in the YXZ convention: when decomposing, 146 | /// first Z, then X, and Y last) corresponding to the rotation 147 | /// represented by the unit quaternion. Returned vector contains 148 | /// the rotation angles in the format (X angle, Y angle, Z angle). 149 | /// 150 | /// The Euler angle representation of this quaternion. 151 | public Vector3d GetEuler() 152 | { 153 | #if DEBUG 154 | if (!IsNormalized()) 155 | { 156 | throw new InvalidOperationException("Quatd is not normalized"); 157 | } 158 | #endif 159 | var basis = new Basisd(this); 160 | return basis.GetEuler(); 161 | } 162 | 163 | /// 164 | /// Returns the inverse of the quaternion. 165 | /// 166 | /// The inverse quaternion. 167 | public Quatd Inverse() 168 | { 169 | #if DEBUG 170 | if (!IsNormalized()) 171 | { 172 | throw new InvalidOperationException("Quatd is not normalized"); 173 | } 174 | #endif 175 | return new Quatd(-x, -y, -z, w); 176 | } 177 | 178 | /// 179 | /// Returns whether the quaternion is normalized or not. 180 | /// 181 | /// A bool for whether the quaternion is normalized or not. 182 | public bool IsNormalized() 183 | { 184 | return Mathd.Abs(LengthSquared - 1) <= Mathd.Epsilon; 185 | } 186 | 187 | /// 188 | /// Returns a copy of the quaternion, normalized to unit length. 189 | /// 190 | /// The normalized quaternion. 191 | public Quatd Normalized() 192 | { 193 | return this / Length; 194 | } 195 | 196 | /// 197 | /// Returns the result of the spherical linear interpolation between 198 | /// this quaternion and `to` by amount `weight`. 199 | /// 200 | /// Note: Both quaternions must be normalized. 201 | /// 202 | /// The destination quaternion for interpolation. Must be normalized. 203 | /// A value on the range of 0.0 to 1.0, representing the amount of interpolation. 204 | /// The resulting quaternion of the interpolation. 205 | public Quatd Slerp(Quatd to, double weight) 206 | { 207 | #if DEBUG 208 | if (!IsNormalized()) 209 | { 210 | throw new InvalidOperationException("Quatd is not normalized"); 211 | } 212 | if (!to.IsNormalized()) 213 | { 214 | throw new ArgumentException("Argument is not normalized", nameof(to)); 215 | } 216 | #endif 217 | 218 | // Calculate cosine. 219 | double cosom = x * to.x + y * to.y + z * to.z + w * to.w; 220 | 221 | var to1 = new Quatd(); 222 | 223 | // Adjust signs if necessary. 224 | if (cosom < 0.0) 225 | { 226 | cosom = -cosom; 227 | to1.x = -to.x; 228 | to1.y = -to.y; 229 | to1.z = -to.z; 230 | to1.w = -to.w; 231 | } 232 | else 233 | { 234 | to1.x = to.x; 235 | to1.y = to.y; 236 | to1.z = to.z; 237 | to1.w = to.w; 238 | } 239 | 240 | double sinom, scale0, scale1; 241 | 242 | // Calculate coefficients. 243 | if (1.0 - cosom > Mathd.Epsilon) 244 | { 245 | // Standard case (Slerp). 246 | double omega = Mathd.Acos(cosom); 247 | sinom = Mathd.Sin(omega); 248 | scale0 = Mathd.Sin((1.0f - weight) * omega) / sinom; 249 | scale1 = Mathd.Sin(weight * omega) / sinom; 250 | } 251 | else 252 | { 253 | // Quaternions are very close so we can do a linear interpolation. 254 | scale0 = 1.0f - weight; 255 | scale1 = weight; 256 | } 257 | 258 | // Calculate final values. 259 | return new Quatd 260 | ( 261 | scale0 * x + scale1 * to1.x, 262 | scale0 * y + scale1 * to1.y, 263 | scale0 * z + scale1 * to1.z, 264 | scale0 * w + scale1 * to1.w 265 | ); 266 | } 267 | 268 | /// 269 | /// Returns the result of the spherical linear interpolation between 270 | /// this quaternion and `to` by amount `weight`, but without 271 | /// checking if the rotation path is not bigger than 90 degrees. 272 | /// 273 | /// The destination quaternion for interpolation. Must be normalized. 274 | /// A value on the range of 0.0 to 1.0, representing the amount of interpolation. 275 | /// The resulting quaternion of the interpolation. 276 | public Quatd Slerpni(Quatd to, double weight) 277 | { 278 | double dot = Dot(to); 279 | 280 | if (Mathd.Abs(dot) > 0.9999f) 281 | { 282 | return this; 283 | } 284 | 285 | double theta = Mathd.Acos(dot); 286 | double sinT = 1.0f / Mathd.Sin(theta); 287 | double newFactor = Mathd.Sin(weight * theta) * sinT; 288 | double invFactor = Mathd.Sin((1.0f - weight) * theta) * sinT; 289 | 290 | return new Quatd 291 | ( 292 | invFactor * x + newFactor * to.x, 293 | invFactor * y + newFactor * to.y, 294 | invFactor * z + newFactor * to.z, 295 | invFactor * w + newFactor * to.w 296 | ); 297 | } 298 | 299 | /// 300 | /// Returns a vector transformed (multiplied) by this quaternion. 301 | /// 302 | /// A vector to transform. 303 | /// The transformed vector. 304 | public Vector3d Xform(Vector3d v) 305 | { 306 | #if DEBUG 307 | if (!IsNormalized()) 308 | { 309 | throw new InvalidOperationException("Quatd is not normalized"); 310 | } 311 | #endif 312 | var u = new Vector3d(x, y, z); 313 | Vector3d uv = u.Cross(v); 314 | return v + ((uv * w) + u.Cross(uv)) * 2; 315 | } 316 | 317 | // Constants 318 | private static readonly Quatd _identity = new Quatd(0, 0, 0, 1); 319 | 320 | /// 321 | /// The identity quaternion, representing no rotation. 322 | /// Equivalent to an identity matrix. If a vector is transformed by 323 | /// an identity quaternion, it will not change. 324 | /// 325 | /// Equivalent to `new Quatd(0, 0, 0, 1)`. 326 | public static Quatd Identity { get { return _identity; } } 327 | 328 | /// 329 | /// Constructs a quaternion defined by the given values. 330 | /// 331 | /// X component of the quaternion (imaginary `i` axis part). 332 | /// Y component of the quaternion (imaginary `j` axis part). 333 | /// Z component of the quaternion (imaginary `k` axis part). 334 | /// W component of the quaternion (real part). 335 | public Quatd(double x, double y, double z, double w) 336 | { 337 | this.x = x; 338 | this.y = y; 339 | this.z = z; 340 | this.w = w; 341 | } 342 | 343 | /// 344 | /// Constructs a quaternion from the given quaternion. 345 | /// 346 | /// The existing quaternion. 347 | public Quatd(Quatd q) 348 | { 349 | this = q; 350 | } 351 | 352 | /// 353 | /// Constructs a quaternion from the given . 354 | /// 355 | /// The basis to construct from. 356 | public Quatd(Basisd basis) 357 | { 358 | this = basis.Quat(); 359 | } 360 | 361 | /// 362 | /// Constructs a quaternion that will perform a rotation specified by 363 | /// Euler angles (in the YXZ convention: when decomposing, 364 | /// first Z, then X, and Y last), 365 | /// given in the vector format as (X angle, Y angle, Z angle). 366 | /// 367 | /// 368 | public Quatd(Vector3d eulerYXZ) 369 | { 370 | double half_a1 = eulerYXZ.y * 0.5f; 371 | double half_a2 = eulerYXZ.x * 0.5f; 372 | double half_a3 = eulerYXZ.z * 0.5f; 373 | 374 | // R = Y(a1).X(a2).Z(a3) convention for Euler angles. 375 | // Conversion to quaternion as listed in https://ntrs.nasa.gov/archive/nasa/casi.ntrs.nasa.gov/19770024290.pdf (page A-6) 376 | // a3 is the angle of the first rotation, following the notation in this reference. 377 | 378 | double cos_a1 = Mathd.Cos(half_a1); 379 | double sin_a1 = Mathd.Sin(half_a1); 380 | double cos_a2 = Mathd.Cos(half_a2); 381 | double sin_a2 = Mathd.Sin(half_a2); 382 | double cos_a3 = Mathd.Cos(half_a3); 383 | double sin_a3 = Mathd.Sin(half_a3); 384 | 385 | x = sin_a1 * cos_a2 * sin_a3 + cos_a1 * sin_a2 * cos_a3; 386 | y = sin_a1 * cos_a2 * cos_a3 - cos_a1 * sin_a2 * sin_a3; 387 | z = cos_a1 * cos_a2 * sin_a3 - sin_a1 * sin_a2 * cos_a3; 388 | w = sin_a1 * sin_a2 * sin_a3 + cos_a1 * cos_a2 * cos_a3; 389 | } 390 | 391 | /// 392 | /// Constructs a quaternion that will rotate around the given axis 393 | /// by the specified angle. The axis must be a normalized vector. 394 | /// 395 | /// The axis to rotate around. Must be normalized. 396 | /// The angle to rotate, in radians. 397 | public Quatd(Vector3d axis, double angle) 398 | { 399 | #if DEBUG 400 | if (!axis.IsNormalized()) 401 | { 402 | throw new ArgumentException("Argument is not normalized", nameof(axis)); 403 | } 404 | #endif 405 | 406 | double d = axis.Length(); 407 | 408 | if (d == 0f) 409 | { 410 | x = 0f; 411 | y = 0f; 412 | z = 0f; 413 | w = 0f; 414 | } 415 | else 416 | { 417 | double sinAngle = Mathd.Sin(angle * 0.5f); 418 | double cosAngle = Mathd.Cos(angle * 0.5f); 419 | double s = sinAngle / d; 420 | 421 | x = axis.x * s; 422 | y = axis.y * s; 423 | z = axis.z * s; 424 | w = cosAngle; 425 | } 426 | } 427 | 428 | #if GODOT 429 | public static explicit operator Godot.Quat(Quatd value) 430 | { 431 | return new Godot.Quat((real_t)value.x, (real_t)value.y, (real_t)value.z, (real_t)value.w); 432 | } 433 | 434 | public static implicit operator Quatd(Godot.Quat value) 435 | { 436 | return new Quatd(value.x, value.y, value.z, value.w); 437 | } 438 | #elif UNITY_5_3_OR_NEWER 439 | // Unity uses a left-handed coordinate system, so we need to be consistent with that for sanity. 440 | // Most code in this struct should just expect the "same" handedness and not any particular one. 441 | // Buuuuuuuuuuuuuuut... there's PROBABLY something in this file that is completely broken. 442 | public static explicit operator UnityEngine.Quaternion(Quatd value) 443 | { 444 | return new UnityEngine.Quaternion((real_t)value.x, (real_t)value.y, (real_t)value.z, (real_t)value.w); 445 | } 446 | 447 | public static implicit operator Quatd(UnityEngine.Quaternion value) 448 | { 449 | return new Quatd(value.x, value.y, value.z, value.w); 450 | } 451 | #endif 452 | 453 | public static Quatd operator *(Quatd left, Quatd right) 454 | { 455 | return new Quatd 456 | ( 457 | left.w * right.x + left.x * right.w + left.y * right.z - left.z * right.y, 458 | left.w * right.y + left.y * right.w + left.z * right.x - left.x * right.z, 459 | left.w * right.z + left.z * right.w + left.x * right.y - left.y * right.x, 460 | left.w * right.w - left.x * right.x - left.y * right.y - left.z * right.z 461 | ); 462 | } 463 | 464 | public static Quatd operator +(Quatd left, Quatd right) 465 | { 466 | return new Quatd(left.x + right.x, left.y + right.y, left.z + right.z, left.w + right.w); 467 | } 468 | 469 | public static Quatd operator -(Quatd left, Quatd right) 470 | { 471 | return new Quatd(left.x - right.x, left.y - right.y, left.z - right.z, left.w - right.w); 472 | } 473 | 474 | public static Quatd operator -(Quatd left) 475 | { 476 | return new Quatd(-left.x, -left.y, -left.z, -left.w); 477 | } 478 | 479 | public static Quatd operator *(Quatd left, Vector3d right) 480 | { 481 | return new Quatd 482 | ( 483 | left.w * right.x + left.y * right.z - left.z * right.y, 484 | left.w * right.y + left.z * right.x - left.x * right.z, 485 | left.w * right.z + left.x * right.y - left.y * right.x, 486 | -left.x * right.x - left.y * right.y - left.z * right.z 487 | ); 488 | } 489 | 490 | public static Quatd operator *(Vector3d left, Quatd right) 491 | { 492 | return new Quatd 493 | ( 494 | right.w * left.x + right.y * left.z - right.z * left.y, 495 | right.w * left.y + right.z * left.x - right.x * left.z, 496 | right.w * left.z + right.x * left.y - right.y * left.x, 497 | -right.x * left.x - right.y * left.y - right.z * left.z 498 | ); 499 | } 500 | 501 | public static Quatd operator *(Quatd left, double right) 502 | { 503 | return new Quatd(left.x * right, left.y * right, left.z * right, left.w * right); 504 | } 505 | 506 | public static Quatd operator *(double left, Quatd right) 507 | { 508 | return new Quatd(right.x * left, right.y * left, right.z * left, right.w * left); 509 | } 510 | 511 | public static Quatd operator /(Quatd left, double right) 512 | { 513 | return left * (1.0f / right); 514 | } 515 | 516 | public static bool operator ==(Quatd left, Quatd right) 517 | { 518 | return left.Equals(right); 519 | } 520 | 521 | public static bool operator !=(Quatd left, Quatd right) 522 | { 523 | return !left.Equals(right); 524 | } 525 | 526 | public override bool Equals(object obj) 527 | { 528 | if (obj is Quatd) 529 | { 530 | return Equals((Quatd)obj); 531 | } 532 | 533 | return false; 534 | } 535 | 536 | public bool Equals(Quatd other) 537 | { 538 | return x == other.x && y == other.y && z == other.z && w == other.w; 539 | } 540 | 541 | /// 542 | /// Returns true if this quaternion and `other` are approximately equal, by running 543 | /// on each component. 544 | /// 545 | /// The other quaternion to compare. 546 | /// Whether or not the quaternions are approximately equal. 547 | public bool IsEqualApprox(Quatd other) 548 | { 549 | return Mathd.IsEqualApprox(x, other.x) && Mathd.IsEqualApprox(y, other.y) && Mathd.IsEqualApprox(z, other.z) && Mathd.IsEqualApprox(w, other.w); 550 | } 551 | 552 | public override int GetHashCode() 553 | { 554 | return y.GetHashCode() ^ x.GetHashCode() ^ z.GetHashCode() ^ w.GetHashCode(); 555 | } 556 | 557 | public override string ToString() 558 | { 559 | return String.Format("({0}, {1}, {2}, {3})", x.ToString(), y.ToString(), z.ToString(), w.ToString()); 560 | } 561 | 562 | public string ToString(string format) 563 | { 564 | return String.Format("({0}, {1}, {2}, {3})", x.ToString(format), y.ToString(format), z.ToString(format), w.ToString(format)); 565 | } 566 | } 567 | } 568 | -------------------------------------------------------------------------------- /addons/extra_math_cs/ExtraMath/Double/Rect2d.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | using System.Runtime.InteropServices; 3 | 4 | namespace ExtraMath 5 | { 6 | /// 7 | /// 2D axis-aligned bounding box. Rect2d consists of a position, a size, and 8 | /// several utility functions. It is typically used for fast overlap tests. 9 | /// 10 | [Serializable] 11 | [StructLayout(LayoutKind.Sequential)] 12 | public struct Rect2d : IEquatable 13 | { 14 | private Vector2d _position; 15 | private Vector2d _size; 16 | 17 | /// 18 | /// Beginning corner. Typically has values lower than End. 19 | /// 20 | /// Directly uses a private field. 21 | public Vector2d Position 22 | { 23 | get { return _position; } 24 | set { _position = value; } 25 | } 26 | 27 | /// 28 | /// Size from Position to End. Typically all components are positive. 29 | /// If the size is negative, you can use to fix it. 30 | /// 31 | /// Directly uses a private field. 32 | public Vector2d Size 33 | { 34 | get { return _size; } 35 | set { _size = value; } 36 | } 37 | 38 | /// 39 | /// Ending corner. This is calculated as plus 40 | /// . Setting this value will change the size. 41 | /// 42 | /// Getting is equivalent to `value = Position + Size`, setting is equivalent to `Size = value - Position`. 43 | public Vector2d End 44 | { 45 | get { return _position + _size; } 46 | set { _size = value - _position; } 47 | } 48 | 49 | /// 50 | /// The area of this Rect2d. 51 | /// 52 | /// Equivalent to . 53 | public double Area 54 | { 55 | get { return GetArea(); } 56 | } 57 | 58 | /// 59 | /// Returns a Rect2d with equivalent position and size, modified so that 60 | /// the top-left corner is the origin and width and height are positive. 61 | /// 62 | /// The modified Rect2d. 63 | public Rect2d Abs() 64 | { 65 | Vector2d end = End; 66 | Vector2d topLeft = new Vector2d(Mathd.Min(_position.x, end.x), Mathd.Min(_position.y, end.y)); 67 | return new Rect2d(topLeft, _size.Abs()); 68 | } 69 | 70 | /// 71 | /// Returns the intersection of this Rect2d and `b`. 72 | /// If the rectangles do not intersect, an empty Rect2d is returned. 73 | /// 74 | /// The other Rect2d. 75 | /// The intersection of this Rect2d and `b`, or an empty Rect2d if they do not intersect. 76 | public Rect2d Clip(Rect2d b) 77 | { 78 | var newRect = b; 79 | 80 | if (!Intersects(newRect)) 81 | { 82 | return new Rect2d(); 83 | } 84 | 85 | newRect._position.x = Mathd.Max(b._position.x, _position.x); 86 | newRect._position.y = Mathd.Max(b._position.y, _position.y); 87 | 88 | Vector2d bEnd = b._position + b._size; 89 | Vector2d end = _position + _size; 90 | 91 | newRect._size.x = Mathd.Min(bEnd.x, end.x) - newRect._position.x; 92 | newRect._size.y = Mathd.Min(bEnd.y, end.y) - newRect._position.y; 93 | 94 | return newRect; 95 | } 96 | 97 | /// 98 | /// Returns true if this Rect2d completely encloses another one. 99 | /// 100 | /// The other Rect2d that may be enclosed. 101 | /// A bool for whether or not this Rect2d encloses `b`. 102 | public bool Encloses(Rect2d b) 103 | { 104 | return b._position.x >= _position.x && b._position.y >= _position.y && 105 | b._position.x + b._size.x < _position.x + _size.x && 106 | b._position.y + b._size.y < _position.y + _size.y; 107 | } 108 | 109 | /// 110 | /// Returns this Rect2d expanded to include a given point. 111 | /// 112 | /// The point to include. 113 | /// The expanded Rect2d. 114 | public Rect2d Expand(Vector2d to) 115 | { 116 | var expanded = this; 117 | 118 | Vector2d begin = expanded._position; 119 | Vector2d end = expanded._position + expanded._size; 120 | 121 | if (to.x < begin.x) 122 | { 123 | begin.x = to.x; 124 | } 125 | if (to.y < begin.y) 126 | { 127 | begin.y = to.y; 128 | } 129 | 130 | if (to.x > end.x) 131 | { 132 | end.x = to.x; 133 | } 134 | if (to.y > end.y) 135 | { 136 | end.y = to.y; 137 | } 138 | 139 | expanded._position = begin; 140 | expanded._size = end - begin; 141 | 142 | return expanded; 143 | } 144 | 145 | /// 146 | /// Returns the area of the Rect2d. 147 | /// 148 | /// The area. 149 | public double GetArea() 150 | { 151 | return _size.x * _size.y; 152 | } 153 | 154 | /// 155 | /// Returns a copy of the Rect2d grown by the specified amount on all sides. 156 | /// 157 | /// The amount to grow by. 158 | /// The grown Rect2d. 159 | public Rect2d Grow(double by) 160 | { 161 | var g = this; 162 | 163 | g._position.x -= by; 164 | g._position.y -= by; 165 | g._size.x += 2 * by; 166 | g._size.y += 2 * by; 167 | 168 | return g; 169 | } 170 | 171 | /// 172 | /// Returns a copy of the Rect2d grown by the specified amount on each side individually. 173 | /// 174 | /// The amount to grow by on the left side. 175 | /// The amount to grow by on the top side. 176 | /// The amount to grow by on the right side. 177 | /// The amount to grow by on the bottom side. 178 | /// The grown Rect2d. 179 | public Rect2d GrowIndividual(double left, double top, double right, double bottom) 180 | { 181 | var g = this; 182 | 183 | g._position.x -= left; 184 | g._position.y -= top; 185 | g._size.x += left + right; 186 | g._size.y += top + bottom; 187 | 188 | return g; 189 | } 190 | 191 | #if GODOT 192 | /// 193 | /// Returns a copy of the Rect2d grown by the specified amount on the specified Side. 194 | /// 195 | /// The side to grow. 196 | /// The amount to grow by. 197 | /// The grown Rect2d. 198 | public Rect2d GrowMargin(Godot.Margin margin, double by) 199 | { 200 | var g = this; 201 | 202 | g = g.GrowIndividual(Godot.Margin.Left == margin ? by : 0, 203 | Godot.Margin.Top == margin ? by : 0, 204 | Godot.Margin.Right == margin ? by : 0, 205 | Godot.Margin.Bottom == margin ? by : 0); 206 | 207 | return g; 208 | } 209 | 210 | /// 211 | /// Returns a copy of the Rect2d grown by the specified amount on the specified Side. 212 | /// 213 | /// The side to grow. 214 | /// The amount to grow by. 215 | /// The grown Rect2d. 216 | public Rect2d GrowSide(Godot.Margin side, double by) 217 | { 218 | var g = this; 219 | 220 | g = g.GrowIndividual(Godot.Margin.Left == side ? by : 0, 221 | Godot.Margin.Top == side ? by : 0, 222 | Godot.Margin.Right == side ? by : 0, 223 | Godot.Margin.Bottom == side ? by : 0); 224 | 225 | return g; 226 | } 227 | #endif // GODOT 228 | 229 | /// 230 | /// Returns true if the Rect2d is flat or empty, or false otherwise. 231 | /// 232 | /// A bool for whether or not the Rect2d has area. 233 | public bool HasNoArea() 234 | { 235 | return _size.x <= 0 || _size.y <= 0; 236 | } 237 | 238 | /// 239 | /// Returns true if the Rect2d contains a point, or false otherwise. 240 | /// 241 | /// The point to check. 242 | /// A bool for whether or not the Rect2d contains `point`. 243 | public bool HasPoint(Vector2d point) 244 | { 245 | if (point.x < _position.x) 246 | return false; 247 | if (point.y < _position.y) 248 | return false; 249 | 250 | if (point.x >= _position.x + _size.x) 251 | return false; 252 | if (point.y >= _position.y + _size.y) 253 | return false; 254 | 255 | return true; 256 | } 257 | 258 | /// 259 | /// Returns the intersection of this Rect2d and `b`. 260 | /// If the rectangles do not intersect, an empty Rect2d is returned. 261 | /// 262 | /// The other Rect2d. 263 | /// The intersection of this Rect2d and `b`, or an empty Rect2d if they do not intersect. 264 | public Rect2d Intersection(Rect2d b) 265 | { 266 | var newRect = b; 267 | 268 | if (!Intersects(newRect)) 269 | { 270 | return new Rect2d(); 271 | } 272 | 273 | newRect._position.x = Mathd.Max(b._position.x, _position.x); 274 | newRect._position.y = Mathd.Max(b._position.y, _position.y); 275 | 276 | Vector2d bEnd = b._position + b._size; 277 | Vector2d end = _position + _size; 278 | 279 | newRect._size.x = Mathd.Min(bEnd.x, end.x) - newRect._position.x; 280 | newRect._size.y = Mathd.Min(bEnd.y, end.y) - newRect._position.y; 281 | 282 | return newRect; 283 | } 284 | 285 | /// 286 | /// Returns true if the Rect2d overlaps with `b` 287 | /// (i.e. they have at least one point in common). 288 | /// 289 | /// If `includeBorders` is true, they will also be considered overlapping 290 | /// if their borders touch, even without intersection. 291 | /// 292 | /// The other Rect2d to check for intersections with. 293 | /// Whether or not to consider borders. 294 | /// A bool for whether or not they are intersecting. 295 | public bool Intersects(Rect2d b, bool includeBorders = false) 296 | { 297 | if (includeBorders) 298 | { 299 | if (_position.x > b._position.x + b._size.x) 300 | { 301 | return false; 302 | } 303 | if (_position.x + _size.x < b._position.x) 304 | { 305 | return false; 306 | } 307 | if (_position.y > b._position.y + b._size.y) 308 | { 309 | return false; 310 | } 311 | if (_position.y + _size.y < b._position.y) 312 | { 313 | return false; 314 | } 315 | } 316 | else 317 | { 318 | if (_position.x >= b._position.x + b._size.x) 319 | { 320 | return false; 321 | } 322 | if (_position.x + _size.x <= b._position.x) 323 | { 324 | return false; 325 | } 326 | if (_position.y >= b._position.y + b._size.y) 327 | { 328 | return false; 329 | } 330 | if (_position.y + _size.y <= b._position.y) 331 | { 332 | return false; 333 | } 334 | } 335 | 336 | return true; 337 | } 338 | 339 | /// 340 | /// Returns a larger Rect2d that contains this Rect2d and `b`. 341 | /// 342 | /// The other Rect2d. 343 | /// The merged Rect2d. 344 | public Rect2d Merge(Rect2d b) 345 | { 346 | Rect2d newRect; 347 | 348 | newRect._position.x = Mathd.Min(b._position.x, _position.x); 349 | newRect._position.y = Mathd.Min(b._position.y, _position.y); 350 | 351 | newRect._size.x = Mathd.Max(b._position.x + b._size.x, _position.x + _size.x); 352 | newRect._size.y = Mathd.Max(b._position.y + b._size.y, _position.y + _size.y); 353 | 354 | newRect._size -= newRect._position; // Make relative again 355 | 356 | return newRect; 357 | } 358 | 359 | /// 360 | /// Constructs a Rect2d from a position and size. 361 | /// 362 | /// The position. 363 | /// The size. 364 | public Rect2d(Vector2d position, Vector2d size) 365 | { 366 | _position = position; 367 | _size = size; 368 | } 369 | 370 | /// 371 | /// Constructs a Rect2d from a position, width, and height. 372 | /// 373 | /// The position. 374 | /// The width. 375 | /// The height. 376 | public Rect2d(Vector2d position, double width, double height) 377 | { 378 | _position = position; 379 | _size = new Vector2d(width, height); 380 | } 381 | 382 | /// 383 | /// Constructs a Rect2d from x, y, and size. 384 | /// 385 | /// The position's X coordinate. 386 | /// The position's Y coordinate. 387 | /// The size. 388 | public Rect2d(double x, double y, Vector2d size) 389 | { 390 | _position = new Vector2d(x, y); 391 | _size = size; 392 | } 393 | 394 | /// 395 | /// Constructs a Rect2d from x, y, width, and height. 396 | /// 397 | /// The position's X coordinate. 398 | /// The position's Y coordinate. 399 | /// The width. 400 | /// The height. 401 | public Rect2d(double x, double y, double width, double height) 402 | { 403 | _position = new Vector2d(x, y); 404 | _size = new Vector2d(width, height); 405 | } 406 | 407 | #if GODOT 408 | public static explicit operator Godot.Rect2(Rect2d value) 409 | { 410 | return new Godot.Rect2((Godot.Vector2)value.Position, (Godot.Vector2)value.Size); 411 | } 412 | 413 | public static implicit operator Rect2d(Godot.Rect2 value) 414 | { 415 | return new Rect2d(value.Position, value.Size); 416 | } 417 | #elif UNITY_5_3_OR_NEWER 418 | public static explicit operator UnityEngine.Rect(Rect2d value) 419 | { 420 | return new UnityEngine.Rect((UnityEngine.Vector2)value.Position, (UnityEngine.Vector2)value.Size); 421 | } 422 | 423 | public static implicit operator Rect2d(UnityEngine.Rect value) 424 | { 425 | return new Rect2d(value.position, value.size); 426 | } 427 | #endif 428 | 429 | public static explicit operator Rect2i(Rect2d value) 430 | { 431 | return new Rect2i((Vector2i)value.Position, (Vector2i)value.Size); 432 | } 433 | 434 | public static implicit operator Rect2d(Rect2i value) 435 | { 436 | return new Rect2d(value.Position, value.Size); 437 | } 438 | 439 | public static bool operator ==(Rect2d left, Rect2d right) 440 | { 441 | return left.Equals(right); 442 | } 443 | 444 | public static bool operator !=(Rect2d left, Rect2d right) 445 | { 446 | return !left.Equals(right); 447 | } 448 | 449 | public override bool Equals(object obj) 450 | { 451 | if (obj is Rect2d) 452 | { 453 | return Equals((Rect2d)obj); 454 | } 455 | 456 | return false; 457 | } 458 | 459 | public bool Equals(Rect2d other) 460 | { 461 | return _position.Equals(other._position) && _size.Equals(other._size); 462 | } 463 | 464 | /// 465 | /// Returns true if this Rect2d and `other` are approximately equal, by running 466 | /// on each component. 467 | /// 468 | /// The other Rect2d to compare. 469 | /// Whether or not the Rect2ds are approximately equal. 470 | public bool IsEqualApprox(Rect2d other) 471 | { 472 | return _position.IsEqualApprox(other._position) && _size.IsEqualApprox(other.Size); 473 | } 474 | 475 | public override int GetHashCode() 476 | { 477 | return _position.GetHashCode() ^ _size.GetHashCode(); 478 | } 479 | 480 | public override string ToString() 481 | { 482 | return String.Format("({0}, {1})", new object[] 483 | { 484 | _position.ToString(), 485 | _size.ToString() 486 | }); 487 | } 488 | 489 | public string ToString(string format) 490 | { 491 | return String.Format("({0}, {1})", new object[] 492 | { 493 | _position.ToString(format), 494 | _size.ToString(format) 495 | }); 496 | } 497 | } 498 | } 499 | -------------------------------------------------------------------------------- /addons/extra_math_cs/ExtraMath/Double/Transform25Dd.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | using System.Runtime.InteropServices; 3 | 4 | namespace ExtraMath 5 | { 6 | /// 7 | /// Calculates the 2D transformation from a 3D position and a Basis25D. 8 | /// 9 | [Serializable] 10 | [StructLayout(LayoutKind.Sequential)] 11 | public struct Transform25Dd : IEquatable 12 | { 13 | // Public fields store information that is used to calculate the properties. 14 | 15 | /// 16 | /// Controls how the 3D position is transformed into 2D. 17 | /// 18 | public Basis25Dd basis; 19 | 20 | /// 21 | /// The 3D position of the object. Should be updated on every frame before everything else. 22 | /// 23 | public Vector3d spatialPosition; 24 | 25 | // Public properties calculate on-the-fly. 26 | 27 | /// 28 | /// The 2D transformation of this object. Slower than FlatPosition. 29 | /// 30 | public Transform2Dd FlatTransform 31 | { 32 | get 33 | { 34 | return new Transform2Dd(0, FlatPosition); 35 | } 36 | } 37 | 38 | /// 39 | /// The 2D position of this object. 40 | /// 41 | public Vector2d FlatPosition 42 | { 43 | get 44 | { 45 | Vector2d pos = spatialPosition.x * basis.x; 46 | pos += spatialPosition.y * basis.y; 47 | pos += spatialPosition.z * basis.z; 48 | return pos; 49 | } 50 | } 51 | 52 | // Constructors 53 | public Transform25Dd(Transform25Dd transform25D) 54 | { 55 | basis = transform25D.basis; 56 | spatialPosition = transform25D.spatialPosition; 57 | } 58 | public Transform25Dd(Basis25Dd basis25D) 59 | { 60 | basis = basis25D; 61 | spatialPosition = Vector3d.Zero; 62 | } 63 | public Transform25Dd(Basis25Dd basis25D, Vector3d position3D) 64 | { 65 | basis = basis25D; 66 | spatialPosition = position3D; 67 | } 68 | public Transform25Dd(Vector2d xAxis, Vector2d yAxis, Vector2d zAxis) 69 | { 70 | basis = new Basis25Dd(xAxis, yAxis, zAxis); 71 | spatialPosition = Vector3d.Zero; 72 | } 73 | public Transform25Dd(Vector2d xAxis, Vector2d yAxis, Vector2d zAxis, Vector3d position3D) 74 | { 75 | basis = new Basis25Dd(xAxis, yAxis, zAxis); 76 | spatialPosition = position3D; 77 | } 78 | 79 | public static explicit operator Transform25D(Transform25Dd value) 80 | { 81 | #if GODOT 82 | return new Transform25D((Basis25D)value.basis, (Godot.Vector3)value.spatialPosition); 83 | #elif UNITY_5_3_OR_NEWER 84 | return new Transform25D((Basis25D)value.basis, (UnityEngine.Vector3)value.spatialPosition); 85 | #endif 86 | } 87 | 88 | public static implicit operator Transform25Dd(Transform25D value) 89 | { 90 | return new Transform25Dd(value.basis, value.spatialPosition); 91 | } 92 | 93 | public static bool operator ==(Transform25Dd left, Transform25Dd right) 94 | { 95 | return left.Equals(right); 96 | } 97 | 98 | public static bool operator !=(Transform25Dd left, Transform25Dd right) 99 | { 100 | return !left.Equals(right); 101 | } 102 | 103 | public override bool Equals(object obj) 104 | { 105 | if (obj is Transform25Dd) 106 | { 107 | return Equals((Transform25Dd)obj); 108 | } 109 | return false; 110 | } 111 | 112 | public bool Equals(Transform25Dd other) 113 | { 114 | return basis.Equals(other.basis) && spatialPosition.Equals(other.spatialPosition); 115 | } 116 | 117 | public override int GetHashCode() 118 | { 119 | return basis.GetHashCode() ^ spatialPosition.GetHashCode(); 120 | } 121 | 122 | public override string ToString() 123 | { 124 | string s = String.Format("({0}, {1})", new object[] 125 | { 126 | basis.ToString(), 127 | spatialPosition.ToString() 128 | }); 129 | return s; 130 | } 131 | 132 | public string ToString(string format) 133 | { 134 | string s = String.Format("({0}, {1})", new object[] 135 | { 136 | basis.ToString(format), 137 | spatialPosition.ToString(format) 138 | }); 139 | return s; 140 | } 141 | } 142 | } 143 | -------------------------------------------------------------------------------- /addons/extra_math_cs/ExtraMath/Double/Transform2Dd.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | using System.Runtime.InteropServices; 3 | 4 | namespace ExtraMath 5 | { 6 | /// 7 | /// 2×3 matrix (2 rows, 3 columns) used for 2D linear transformations. 8 | /// It can represent transformations such as translation, rotation, or scaling. 9 | /// It consists of a three values: x, y, and the origin. 10 | /// 11 | /// For more information, read this documentation article: 12 | /// https://docs.godotengine.org/en/latest/tutorials/math/matrices_and_transforms.html 13 | /// 14 | [Serializable] 15 | [StructLayout(LayoutKind.Sequential)] 16 | public struct Transform2Dd : IEquatable 17 | { 18 | /// 19 | /// The basis matrix's X vector (column 0). Equivalent to array index `[0]`. 20 | /// 21 | /// 22 | public Vector2d x; 23 | 24 | /// 25 | /// The basis matrix's Y vector (column 1). Equivalent to array index `[1]`. 26 | /// 27 | public Vector2d y; 28 | 29 | /// 30 | /// The origin vector (column 2, the third column). Equivalent to array index `[2]`. 31 | /// The origin vector represents translation. 32 | /// 33 | public Vector2d origin; 34 | 35 | /// 36 | /// The rotation of this transformation matrix. 37 | /// 38 | /// Getting is equivalent to calling with the values of . 39 | public double Rotation 40 | { 41 | get 42 | { 43 | return Mathd.Atan2(x.y, x.x); 44 | } 45 | set 46 | { 47 | Vector2d scale = Scale; 48 | x.x = y.y = Mathd.Cos(value); 49 | x.y = y.x = Mathd.Sin(value); 50 | y.x *= -1; 51 | Scale = scale; 52 | } 53 | } 54 | 55 | /// 56 | /// The scale of this transformation matrix. 57 | /// 58 | /// Equivalent to the lengths of each column vector, but Y is negative if the determinant is negative. 59 | public Vector2d Scale 60 | { 61 | get 62 | { 63 | double detSign = Mathd.Sign(BasisDeterminant()); 64 | return new Vector2d(x.Length(), detSign * y.Length()); 65 | } 66 | set 67 | { 68 | value /= Scale; // Value becomes what's called "delta_scale" in core. 69 | x *= value.x; 70 | y *= value.y; 71 | } 72 | } 73 | 74 | /// 75 | /// Access whole columns in the form of Vector2d. The third column is the origin vector. 76 | /// 77 | /// Which column vector. 78 | public Vector2d this[int column] 79 | { 80 | get 81 | { 82 | switch (column) 83 | { 84 | case 0: 85 | return x; 86 | case 1: 87 | return y; 88 | case 2: 89 | return origin; 90 | default: 91 | throw new IndexOutOfRangeException(); 92 | } 93 | } 94 | set 95 | { 96 | switch (column) 97 | { 98 | case 0: 99 | x = value; 100 | return; 101 | case 1: 102 | y = value; 103 | return; 104 | case 2: 105 | origin = value; 106 | return; 107 | default: 108 | throw new IndexOutOfRangeException(); 109 | } 110 | } 111 | } 112 | 113 | /// 114 | /// Access matrix elements in column-major order. The third column is the origin vector. 115 | /// 116 | /// Which column, the matrix horizontal position. 117 | /// Which row, the matrix vertical position. 118 | public double this[int column, int row] 119 | { 120 | get 121 | { 122 | return this[column][row]; 123 | } 124 | set 125 | { 126 | Vector2d columnVector = this[column]; 127 | columnVector[row] = value; 128 | this[column] = columnVector; 129 | } 130 | } 131 | 132 | /// 133 | /// Returns the inverse of the transform, under the assumption that 134 | /// the transformation is composed of rotation, scaling, and translation. 135 | /// 136 | /// The inverse transformation matrix. 137 | public Transform2Dd AffineInverse() 138 | { 139 | double det = BasisDeterminant(); 140 | 141 | if (det == 0) 142 | throw new InvalidOperationException("Matrix determinant is zero and cannot be inverted."); 143 | 144 | var inv = this; 145 | 146 | double temp = inv[0, 0]; 147 | inv[0, 0] = inv[1, 1]; 148 | inv[1, 1] = temp; 149 | 150 | double detInv = 1.0f / det; 151 | 152 | inv[0] *= new Vector2d(detInv, -detInv); 153 | inv[1] *= new Vector2d(-detInv, detInv); 154 | 155 | inv[2] = inv.BasisXform(-inv[2]); 156 | 157 | return inv; 158 | } 159 | 160 | /// 161 | /// Returns the determinant of the basis matrix. If the basis is 162 | /// uniformly scaled, its determinant is the square of the scale. 163 | /// 164 | /// A negative determinant means the Y scale is negative. 165 | /// A zero determinant means the basis isn't invertible, 166 | /// and is usually considered invalid. 167 | /// 168 | /// The determinant of the basis matrix. 169 | private double BasisDeterminant() 170 | { 171 | return x.x * y.y - x.y * y.x; 172 | } 173 | 174 | /// 175 | /// Returns a vector transformed (multiplied) by the basis matrix. 176 | /// This method does not account for translation (the origin vector). 177 | /// 178 | /// A vector to transform. 179 | /// The transformed vector. 180 | public Vector2d BasisXform(Vector2d v) 181 | { 182 | return new Vector2d(Tdotx(v), Tdoty(v)); 183 | } 184 | 185 | /// 186 | /// Returns a vector transformed (multiplied) by the inverse basis matrix. 187 | /// This method does not account for translation (the origin vector). 188 | /// 189 | /// Note: This results in a multiplication by the inverse of the 190 | /// basis matrix only if it represents a rotation-reflection. 191 | /// 192 | /// A vector to inversely transform. 193 | /// The inversely transformed vector. 194 | public Vector2d BasisXformInv(Vector2d v) 195 | { 196 | return new Vector2d(x.Dot(v), y.Dot(v)); 197 | } 198 | 199 | /// 200 | /// Interpolates this transform to the other `transform` by `weight`. 201 | /// 202 | /// The other transform. 203 | /// A value on the range of 0.0 to 1.0, representing the amount of interpolation. 204 | /// The interpolated transform. 205 | public Transform2Dd InterpolateWith(Transform2Dd transform, double weight) 206 | { 207 | double r1 = Rotation; 208 | double r2 = transform.Rotation; 209 | 210 | Vector2d s1 = Scale; 211 | Vector2d s2 = transform.Scale; 212 | 213 | // Slerp rotation 214 | var v1 = new Vector2d(Mathd.Cos(r1), Mathd.Sin(r1)); 215 | var v2 = new Vector2d(Mathd.Cos(r2), Mathd.Sin(r2)); 216 | 217 | double dot = v1.Dot(v2); 218 | 219 | dot = Mathd.Clamp(dot, -1.0f, 1.0f); 220 | 221 | Vector2d v; 222 | 223 | if (dot > 0.9995f) 224 | { 225 | // Linearly interpolate to avoid numerical precision issues 226 | v = v1.Lerp(v2, weight).Normalized(); 227 | } 228 | else 229 | { 230 | double angle = weight * Mathd.Acos(dot); 231 | Vector2d v3 = (v2 - v1 * dot).Normalized(); 232 | v = v1 * Mathd.Cos(angle) + v3 * Mathd.Sin(angle); 233 | } 234 | 235 | // Extract parameters 236 | Vector2d p1 = origin; 237 | Vector2d p2 = transform.origin; 238 | 239 | // Construct matrix 240 | var res = new Transform2Dd(Mathd.Atan2(v.y, v.x), p1.Lerp(p2, weight)); 241 | Vector2d scale = s1.Lerp(s2, weight); 242 | res.x *= scale; 243 | res.y *= scale; 244 | 245 | return res; 246 | } 247 | 248 | /// 249 | /// Returns the inverse of the transform, under the assumption that 250 | /// the transformation is composed of rotation and translation 251 | /// (no scaling, use for transforms with scaling). 252 | /// 253 | /// The inverse matrix. 254 | public Transform2Dd Inverse() 255 | { 256 | var inv = this; 257 | 258 | // Swap 259 | double temp = inv.x.y; 260 | inv.x.y = inv.y.x; 261 | inv.y.x = temp; 262 | 263 | inv.origin = inv.BasisXform(-inv.origin); 264 | 265 | return inv; 266 | } 267 | 268 | /// 269 | /// Returns the transform with the basis orthogonal (90 degrees), 270 | /// and normalized axis vectors (scale of 1 or -1). 271 | /// 272 | /// The orthonormalized transform. 273 | public Transform2Dd Orthonormalized() 274 | { 275 | var on = this; 276 | 277 | Vector2d onX = on.x; 278 | Vector2d onY = on.y; 279 | 280 | onX.Normalize(); 281 | onY = onY - onX * onX.Dot(onY); 282 | onY.Normalize(); 283 | 284 | on.x = onX; 285 | on.y = onY; 286 | 287 | return on; 288 | } 289 | 290 | /// 291 | /// Rotates the transform by `phi` (in radians), using matrix multiplication. 292 | /// 293 | /// The angle to rotate, in radians. 294 | /// The rotated transformation matrix. 295 | public Transform2Dd Rotated(double phi) 296 | { 297 | return this * new Transform2Dd(phi, new Vector2d()); 298 | } 299 | 300 | /// 301 | /// Scales the transform by the given scaling factor, using matrix multiplication. 302 | /// 303 | /// The scale to introduce. 304 | /// The scaled transformation matrix. 305 | public Transform2Dd Scaled(Vector2d scale) 306 | { 307 | var copy = this; 308 | copy.x *= scale; 309 | copy.y *= scale; 310 | copy.origin *= scale; 311 | return copy; 312 | } 313 | 314 | private void ScaleBasis(Vector2d scale) 315 | { 316 | x.x *= scale.x; 317 | x.y *= scale.y; 318 | y.x *= scale.x; 319 | y.y *= scale.y; 320 | } 321 | 322 | private double Tdotx(Vector2d with) 323 | { 324 | return this[0, 0] * with[0] + this[1, 0] * with[1]; 325 | } 326 | 327 | private double Tdoty(Vector2d with) 328 | { 329 | return this[0, 1] * with[0] + this[1, 1] * with[1]; 330 | } 331 | 332 | /// 333 | /// Translates the transform by the given `offset`, 334 | /// relative to the transform's basis vectors. 335 | /// 336 | /// Unlike and , 337 | /// this does not use matrix multiplication. 338 | /// 339 | /// The offset to translate by. 340 | /// The translated matrix. 341 | public Transform2Dd Translated(Vector2d offset) 342 | { 343 | var copy = this; 344 | copy.origin += copy.BasisXform(offset); 345 | return copy; 346 | } 347 | 348 | /// 349 | /// Returns a vector transformed (multiplied) by this transformation matrix. 350 | /// 351 | /// A vector to transform. 352 | /// The transformed vector. 353 | public Vector2d Xform(Vector2d v) 354 | { 355 | return new Vector2d(Tdotx(v), Tdoty(v)) + origin; 356 | } 357 | 358 | /// 359 | /// Returns a vector transformed (multiplied) by the inverse transformation matrix. 360 | /// 361 | /// A vector to inversely transform. 362 | /// The inversely transformed vector. 363 | public Vector2d XformInv(Vector2d v) 364 | { 365 | Vector2d vInv = v - origin; 366 | return new Vector2d(x.Dot(vInv), y.Dot(vInv)); 367 | } 368 | 369 | // Constants 370 | private static readonly Transform2Dd _identity = new Transform2Dd(1, 0, 0, 1, 0, 0); 371 | private static readonly Transform2Dd _flipX = new Transform2Dd(-1, 0, 0, 1, 0, 0); 372 | private static readonly Transform2Dd _flipY = new Transform2Dd(1, 0, 0, -1, 0, 0); 373 | 374 | /// 375 | /// The identity transform, with no translation, rotation, or scaling applied. 376 | /// This is used as a replacement for `Transform2Dd()` in GDScript. 377 | /// Do not use `new Transform2Dd()` with no arguments in C#, because it sets all values to zero. 378 | /// 379 | /// Equivalent to `new Transform2Dd(Vector2d.Right, Vector2d.Down, Vector2d.Zero)`. 380 | public static Transform2Dd Identity { get { return _identity; } } 381 | /// 382 | /// The transform that will flip something along the X axis. 383 | /// 384 | /// Equivalent to `new Transform2Dd(Vector2d.Left, Vector2d.Down, Vector2d.Zero)`. 385 | public static Transform2Dd FlipX { get { return _flipX; } } 386 | /// 387 | /// The transform that will flip something along the Y axis. 388 | /// 389 | /// Equivalent to `new Transform2Dd(Vector2d.Right, Vector2d.Up, Vector2d.Zero)`. 390 | public static Transform2Dd FlipY { get { return _flipY; } } 391 | 392 | /// 393 | /// Constructs a transformation matrix from 3 vectors (matrix columns). 394 | /// 395 | /// The X vector, or column index 0. 396 | /// The Y vector, or column index 1. 397 | /// The origin vector, or column index 2. 398 | public Transform2Dd(Vector2d xAxis, Vector2d yAxis, Vector2d originPos) 399 | { 400 | x = xAxis; 401 | y = yAxis; 402 | origin = originPos; 403 | } 404 | 405 | /// 406 | /// Constructs a transformation matrix from the given components. 407 | /// Arguments are named such that xy is equal to calling x.y 408 | /// 409 | /// The X component of the X column vector, accessed via `t.x.x` or `[0][0]` 410 | /// The Y component of the X column vector, accessed via `t.x.y` or `[0][1]` 411 | /// The X component of the Y column vector, accessed via `t.y.x` or `[1][0]` 412 | /// The Y component of the Y column vector, accessed via `t.y.y` or `[1][1]` 413 | /// The X component of the origin vector, accessed via `t.origin.x` or `[2][0]` 414 | /// The Y component of the origin vector, accessed via `t.origin.y` or `[2][1]` 415 | public Transform2Dd(double xx, double xy, double yx, double yy, double ox, double oy) 416 | { 417 | x = new Vector2d(xx, xy); 418 | y = new Vector2d(yx, yy); 419 | origin = new Vector2d(ox, oy); 420 | } 421 | 422 | /// 423 | /// Constructs a transformation matrix from a rotation value and origin vector. 424 | /// 425 | /// The rotation of the new transform, in radians. 426 | /// The origin vector, or column index 2. 427 | public Transform2Dd(double rot, Vector2d pos) 428 | { 429 | x.x = y.y = Mathd.Cos(rot); 430 | x.y = y.x = Mathd.Sin(rot); 431 | y.x *= -1; 432 | origin = pos; 433 | } 434 | 435 | #if GODOT 436 | public static explicit operator Godot.Transform2D(Transform2Dd value) 437 | { 438 | return new Godot.Transform2D((Godot.Vector2)value.x, (Godot.Vector2)value.y, (Godot.Vector2)value.origin); 439 | } 440 | 441 | public static implicit operator Transform2Dd(Godot.Transform2D value) 442 | { 443 | return new Transform2Dd(value.x, value.y, value.origin); 444 | } 445 | #elif UNITY_5_3_OR_NEWER 446 | // Transform in Unity is a component, not a struct. 447 | // Operators are not possible, so put methods here instead. 448 | // Use "Apply" and "Store" instead of "Set" and "Get" to be unambiguous. 449 | /// 450 | /// Applies the values of this Transform2Dd struct to the given Transform. 451 | /// 452 | public void ApplyTransform(UnityEngine.Transform transform) 453 | { 454 | transform.localPosition = new UnityEngine.Vector3((float)origin.x, (float)origin.y); 455 | transform.eulerAngles = UnityEngine.Vector3.forward * (float)Rotation; 456 | } 457 | 458 | /// 459 | /// Stores the values of the given Transform in a Transform2Dd struct. 460 | /// 461 | public static Transform2Dd StoreTransform(UnityEngine.Transform transform) 462 | { 463 | double rotation = transform.eulerAngles.z; 464 | Vector3d localPos = transform.localPosition; 465 | Vector2d position = new Vector2d(localPos.x, localPos.y); 466 | return new Transform2Dd(rotation, position); 467 | } 468 | #endif 469 | 470 | public static Transform2Dd operator *(Transform2Dd left, Transform2Dd right) 471 | { 472 | left.origin = left.Xform(right.origin); 473 | 474 | double x0 = left.Tdotx(right.x); 475 | double x1 = left.Tdoty(right.x); 476 | double y0 = left.Tdotx(right.y); 477 | double y1 = left.Tdoty(right.y); 478 | 479 | left.x.x = x0; 480 | left.x.y = x1; 481 | left.y.x = y0; 482 | left.y.y = y1; 483 | 484 | return left; 485 | } 486 | 487 | public static bool operator ==(Transform2Dd left, Transform2Dd right) 488 | { 489 | return left.Equals(right); 490 | } 491 | 492 | public static bool operator !=(Transform2Dd left, Transform2Dd right) 493 | { 494 | return !left.Equals(right); 495 | } 496 | 497 | public override bool Equals(object obj) 498 | { 499 | return obj is Transform2Dd transform2D && Equals(transform2D); 500 | } 501 | 502 | public bool Equals(Transform2Dd other) 503 | { 504 | return x.Equals(other.x) && y.Equals(other.y) && origin.Equals(other.origin); 505 | } 506 | 507 | /// 508 | /// Returns true if this transform and `other` are approximately equal, by running 509 | /// on each component. 510 | /// 511 | /// The other transform to compare. 512 | /// Whether or not the matrices are approximately equal. 513 | public bool IsEqualApprox(Transform2Dd other) 514 | { 515 | return x.IsEqualApprox(other.x) && y.IsEqualApprox(other.y) && origin.IsEqualApprox(other.origin); 516 | } 517 | 518 | public override int GetHashCode() 519 | { 520 | return x.GetHashCode() ^ y.GetHashCode() ^ origin.GetHashCode(); 521 | } 522 | 523 | public override string ToString() 524 | { 525 | return String.Format("({0}, {1}, {2})", new object[] 526 | { 527 | x.ToString(), 528 | y.ToString(), 529 | origin.ToString() 530 | }); 531 | } 532 | 533 | public string ToString(string format) 534 | { 535 | return String.Format("({0}, {1}, {2})", new object[] 536 | { 537 | x.ToString(format), 538 | y.ToString(format), 539 | origin.ToString(format) 540 | }); 541 | } 542 | } 543 | } 544 | -------------------------------------------------------------------------------- /addons/extra_math_cs/ExtraMath/Double/Transformd.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | using System.Runtime.InteropServices; 3 | 4 | namespace ExtraMath 5 | { 6 | /// 7 | /// 3×4 matrix (3 rows, 4 columns) used for 3D linear transformations. 8 | /// It can represent transformations such as translation, rotation, or scaling. 9 | /// It consists of a (first 3 columns) and a 10 | /// for the origin (last column). 11 | /// 12 | /// For more information, read this documentation article: 13 | /// https://docs.godotengine.org/en/latest/tutorials/math/matrices_and_transforms.html 14 | /// 15 | [Serializable] 16 | [StructLayout(LayoutKind.Sequential)] 17 | public struct Transformd : IEquatable 18 | { 19 | /// 20 | /// The of this transform. Contains the X, Y, and Z basis 21 | /// vectors (columns 0 to 2) and is responsible for rotation and scale. 22 | /// 23 | public Basisd basis; 24 | 25 | /// 26 | /// The origin vector (column 3, the fourth column). Equivalent to array index `[3]`. 27 | /// 28 | public Vector3d origin; 29 | 30 | /// 31 | /// Access whole columns in the form of Vector3d. The fourth column is the origin vector. 32 | /// 33 | /// Which column vector. 34 | public Vector3d this[int column] 35 | { 36 | get 37 | { 38 | switch (column) 39 | { 40 | case 0: 41 | return basis.Column0; 42 | case 1: 43 | return basis.Column1; 44 | case 2: 45 | return basis.Column2; 46 | case 3: 47 | return origin; 48 | default: 49 | throw new IndexOutOfRangeException(); 50 | } 51 | } 52 | set 53 | { 54 | switch (column) 55 | { 56 | case 0: 57 | basis.Column0 = value; 58 | return; 59 | case 1: 60 | basis.Column1 = value; 61 | return; 62 | case 2: 63 | basis.Column2 = value; 64 | return; 65 | case 3: 66 | origin = value; 67 | return; 68 | default: 69 | throw new IndexOutOfRangeException(); 70 | } 71 | } 72 | } 73 | 74 | /// 75 | /// Access matrix elements in column-major order. The fourth column is the origin vector. 76 | /// 77 | /// Which column, the matrix horizontal position. 78 | /// Which row, the matrix vertical position. 79 | public double this[int column, int row] 80 | { 81 | get 82 | { 83 | if (column == 3) 84 | { 85 | return origin[row]; 86 | } 87 | return basis[column, row]; 88 | } 89 | set 90 | { 91 | if (column == 3) 92 | { 93 | origin[row] = value; 94 | return; 95 | } 96 | basis[column, row] = value; 97 | } 98 | } 99 | 100 | /// 101 | /// Returns the inverse of the transform, under the assumption that 102 | /// the transformation is composed of rotation, scaling, and translation. 103 | /// 104 | /// The inverse transformation matrix. 105 | public Transformd AffineInverse() 106 | { 107 | Basisd basisInv = basis.Inverse(); 108 | return new Transformd(basisInv, basisInv.Xform(-origin)); 109 | } 110 | 111 | /// 112 | /// Interpolates this transform to the other `transform` by `weight`. 113 | /// 114 | /// The other transform. 115 | /// A value on the range of 0.0 to 1.0, representing the amount of interpolation. 116 | /// The interpolated transform. 117 | public Transformd InterpolateWith(Transformd transform, double weight) 118 | { 119 | /* not sure if very "efficient" but good enough? */ 120 | 121 | Vector3d sourceScale = basis.Scale; 122 | Quatd sourceRotation = basis.RotationQuat(); 123 | Vector3d sourceLocation = origin; 124 | 125 | Vector3d destinationScale = transform.basis.Scale; 126 | Quatd destinationRotation = transform.basis.RotationQuat(); 127 | Vector3d destinationLocation = transform.origin; 128 | 129 | var interpolated = new Transformd(); 130 | interpolated.basis.SetQuatScale(sourceRotation.Slerp(destinationRotation, weight).Normalized(), sourceScale.Lerp(destinationScale, weight)); 131 | interpolated.origin = sourceLocation.Lerp(destinationLocation, weight); 132 | 133 | return interpolated; 134 | } 135 | 136 | /// 137 | /// Returns the inverse of the transform, under the assumption that 138 | /// the transformation is composed of rotation and translation 139 | /// (no scaling, use for transforms with scaling). 140 | /// 141 | /// The inverse matrix. 142 | public Transformd Inverse() 143 | { 144 | Basisd basisTr = basis.Transposed(); 145 | return new Transformd(basisTr, basisTr.Xform(-origin)); 146 | } 147 | 148 | /// 149 | /// Returns a copy of the transform rotated such that its 150 | /// -Z axis (forward) points towards the target position. 151 | /// 152 | /// The transform will first be rotated around the given up vector, 153 | /// and then fully aligned to the target by a further rotation around 154 | /// an axis perpendicular to both the target and up vectors. 155 | /// 156 | /// Operations take place in global space. 157 | /// 158 | /// The object to look at. 159 | /// The relative up direction 160 | /// The resulting transform. 161 | public Transformd LookingAt(Vector3d target, Vector3d up) 162 | { 163 | var t = this; 164 | t.SetLookAt(origin, target, up); 165 | return t; 166 | } 167 | 168 | /// 169 | /// Returns the transform with the basis orthogonal (90 degrees), 170 | /// and normalized axis vectors (scale of 1 or -1). 171 | /// 172 | /// The orthonormalized transform. 173 | public Transformd Orthonormalized() 174 | { 175 | return new Transformd(basis.Orthonormalized(), origin); 176 | } 177 | 178 | /// 179 | /// Rotates the transform around the given `axis` by `phi` (in radians), 180 | /// using matrix multiplication. The axis must be a normalized vector. 181 | /// 182 | /// The axis to rotate around. Must be normalized. 183 | /// The angle to rotate, in radians. 184 | /// The rotated transformation matrix. 185 | public Transformd Rotated(Vector3d axis, double phi) 186 | { 187 | return new Transformd(new Basisd(axis, phi), new Vector3d()) * this; 188 | } 189 | 190 | /// 191 | /// Scales the transform by the given 3D scaling factor, using matrix multiplication. 192 | /// 193 | /// The scale to introduce. 194 | /// The scaled transformation matrix. 195 | public Transformd Scaled(Vector3d scale) 196 | { 197 | return new Transformd(basis.Scaled(scale), origin * scale); 198 | } 199 | 200 | private void SetLookAt(Vector3d eye, Vector3d target, Vector3d up) 201 | { 202 | // Make rotation matrix 203 | // Z vector 204 | Vector3d column2 = eye - target; 205 | 206 | column2.Normalize(); 207 | 208 | Vector3d column1 = up; 209 | 210 | Vector3d column0 = column1.Cross(column2); 211 | 212 | // Recompute Y = Z cross X 213 | column1 = column2.Cross(column0); 214 | 215 | column0.Normalize(); 216 | column1.Normalize(); 217 | 218 | basis = new Basisd(column0, column1, column2); 219 | 220 | origin = eye; 221 | } 222 | 223 | /// 224 | /// Translates the transform by the given `offset`, 225 | /// relative to the transform's basis vectors. 226 | /// 227 | /// Unlike and , 228 | /// this does not use matrix multiplication. 229 | /// 230 | /// The offset to translate by. 231 | /// The translated matrix. 232 | public Transformd Translated(Vector3d offset) 233 | { 234 | return new Transformd(basis, new Vector3d 235 | ( 236 | origin[0] += basis.Row0.Dot(offset), 237 | origin[1] += basis.Row1.Dot(offset), 238 | origin[2] += basis.Row2.Dot(offset) 239 | )); 240 | } 241 | 242 | /// 243 | /// Returns a vector transformed (multiplied) by this transformation matrix. 244 | /// 245 | /// A vector to transform. 246 | /// The transformed vector. 247 | public Vector3d Xform(Vector3d v) 248 | { 249 | return new Vector3d 250 | ( 251 | basis.Row0.Dot(v) + origin.x, 252 | basis.Row1.Dot(v) + origin.y, 253 | basis.Row2.Dot(v) + origin.z 254 | ); 255 | } 256 | 257 | /// 258 | /// Returns a vector transformed (multiplied) by the transposed transformation matrix. 259 | /// 260 | /// Note: This results in a multiplication by the inverse of the 261 | /// transformation matrix only if it represents a rotation-reflection. 262 | /// 263 | /// A vector to inversely transform. 264 | /// The inversely transformed vector. 265 | public Vector3d XformInv(Vector3d v) 266 | { 267 | Vector3d vInv = v - origin; 268 | 269 | return new Vector3d 270 | ( 271 | basis.Row0[0] * vInv.x + basis.Row1[0] * vInv.y + basis.Row2[0] * vInv.z, 272 | basis.Row0[1] * vInv.x + basis.Row1[1] * vInv.y + basis.Row2[1] * vInv.z, 273 | basis.Row0[2] * vInv.x + basis.Row1[2] * vInv.y + basis.Row2[2] * vInv.z 274 | ); 275 | } 276 | 277 | // Constants 278 | private static readonly Transformd _identity = new Transformd(Basisd.Identity, Vector3d.Zero); 279 | private static readonly Transformd _flipX = new Transformd(new Basisd(-1, 0, 0, 0, 1, 0, 0, 0, 1), Vector3d.Zero); 280 | private static readonly Transformd _flipY = new Transformd(new Basisd(1, 0, 0, 0, -1, 0, 0, 0, 1), Vector3d.Zero); 281 | private static readonly Transformd _flipZ = new Transformd(new Basisd(1, 0, 0, 0, 1, 0, 0, 0, -1), Vector3d.Zero); 282 | 283 | /// 284 | /// The identity transform, with no translation, rotation, or scaling applied. 285 | /// This is used as a replacement for `Transformd()` in GDScript. 286 | /// Do not use `new Transformd()` with no arguments in C#, because it sets all values to zero. 287 | /// 288 | /// Equivalent to `new Transformd(Vector3d.Right, Vector3d.Up, Vector3d.Back, Vector3d.Zero)`. 289 | public static Transformd Identity { get { return _identity; } } 290 | /// 291 | /// The transform that will flip something along the X axis. 292 | /// 293 | /// Equivalent to `new Transformd(Vector3d.Left, Vector3d.Up, Vector3d.Back, Vector3d.Zero)`. 294 | public static Transformd FlipX { get { return _flipX; } } 295 | /// 296 | /// The transform that will flip something along the Y axis. 297 | /// 298 | /// Equivalent to `new Transformd(Vector3d.Right, Vector3d.Down, Vector3d.Back, Vector3d.Zero)`. 299 | public static Transformd FlipY { get { return _flipY; } } 300 | /// 301 | /// The transform that will flip something along the Z axis. 302 | /// 303 | /// Equivalent to `new Transformd(Vector3d.Right, Vector3d.Up, Vector3d.Forward, Vector3d.Zero)`. 304 | public static Transformd FlipZ { get { return _flipZ; } } 305 | 306 | /// 307 | /// Constructs a transformation matrix from 4 vectors (matrix columns). 308 | /// 309 | /// The X vector, or column index 0. 310 | /// The Y vector, or column index 1. 311 | /// The Z vector, or column index 2. 312 | /// The origin vector, or column index 3. 313 | public Transformd(Vector3d column0, Vector3d column1, Vector3d column2, Vector3d origin) 314 | { 315 | basis = new Basisd(column0, column1, column2); 316 | this.origin = origin; 317 | } 318 | 319 | /// 320 | /// Constructs a transformation matrix from the given quaternion and origin vector. 321 | /// 322 | /// The to create the basis from. 323 | /// The origin vector, or column index 3. 324 | public Transformd(Quatd quat, Vector3d origin) 325 | { 326 | basis = new Basisd(quat); 327 | this.origin = origin; 328 | } 329 | 330 | /// 331 | /// Constructs a transformation matrix from the given basis and origin vector. 332 | /// 333 | /// The to create the basis from. 334 | /// The origin vector, or column index 3. 335 | public Transformd(Basisd basis, Vector3d origin) 336 | { 337 | this.basis = basis; 338 | this.origin = origin; 339 | } 340 | 341 | #if GODOT 342 | public static explicit operator Godot.Transform(Transformd value) 343 | { 344 | return new Godot.Transform((Godot.Basis)value.basis, (Godot.Vector3)value.origin); 345 | } 346 | 347 | public static implicit operator Transformd(Godot.Transform value) 348 | { 349 | return new Transformd(value.basis, value.origin); 350 | } 351 | #elif UNITY_5_3_OR_NEWER 352 | // Transform in Unity is a component, not a struct. 353 | // Operators are not possible, so put methods here instead. 354 | // Use "Apply" and "Store" instead of "Set" and "Get" to be unambiguous. 355 | /// 356 | /// Applies the values of this Transformd struct to the given Transform. Note: Very inefficient. 357 | /// 358 | public void ApplyTransform(UnityEngine.Transform transform) 359 | { 360 | transform.localPosition = (UnityEngine.Vector3)origin; 361 | transform.localRotation = (UnityEngine.Quaternion)basis.Quat(); 362 | } 363 | 364 | /// 365 | /// Stores the values of the given Transform in a Transformd struct. Note: Very inefficient. 366 | /// 367 | public static Transformd StoreTransform(UnityEngine.Transform transform) 368 | { 369 | Quatd rotation = transform.localRotation; 370 | Vector3d position = transform.localPosition; 371 | return new Transformd(rotation, position); 372 | } 373 | #endif 374 | 375 | public static Transformd operator *(Transformd left, Transformd right) 376 | { 377 | left.origin = left.Xform(right.origin); 378 | left.basis *= right.basis; 379 | return left; 380 | } 381 | 382 | public static bool operator ==(Transformd left, Transformd right) 383 | { 384 | return left.Equals(right); 385 | } 386 | 387 | public static bool operator !=(Transformd left, Transformd right) 388 | { 389 | return !left.Equals(right); 390 | } 391 | 392 | public override bool Equals(object obj) 393 | { 394 | if (obj is Transformd) 395 | { 396 | return Equals((Transformd)obj); 397 | } 398 | 399 | return false; 400 | } 401 | 402 | public bool Equals(Transformd other) 403 | { 404 | return basis.Equals(other.basis) && origin.Equals(other.origin); 405 | } 406 | 407 | /// 408 | /// Returns true if this transform and `other` are approximately equal, by running 409 | /// on each component. 410 | /// 411 | /// The other transform to compare. 412 | /// Whether or not the matrices are approximately equal. 413 | public bool IsEqualApprox(Transformd other) 414 | { 415 | return basis.IsEqualApprox(other.basis) && origin.IsEqualApprox(other.origin); 416 | } 417 | 418 | public override int GetHashCode() 419 | { 420 | return basis.GetHashCode() ^ origin.GetHashCode(); 421 | } 422 | 423 | public override string ToString() 424 | { 425 | return String.Format("{0} - {1}", new object[] 426 | { 427 | basis.ToString(), 428 | origin.ToString() 429 | }); 430 | } 431 | 432 | public string ToString(string format) 433 | { 434 | return String.Format("{0} - {1}", new object[] 435 | { 436 | basis.ToString(format), 437 | origin.ToString(format) 438 | }); 439 | } 440 | } 441 | } 442 | -------------------------------------------------------------------------------- /addons/extra_math_cs/ExtraMath/Double/Vector4d.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | using System.Runtime.InteropServices; 3 | 4 | #if GODOT_REAL_T_IS_DOUBLE 5 | using real_t = System.Double; 6 | #else 7 | using real_t = System.Single; 8 | #endif 9 | 10 | namespace ExtraMath 11 | { 12 | /// 13 | /// 4-element structure that can be used to represent positions in 4D space or any other pair of numeric values. 14 | /// 15 | [Serializable] 16 | [StructLayout(LayoutKind.Sequential)] 17 | public struct Vector4d : IEquatable 18 | { 19 | public enum Axis 20 | { 21 | X = 0, 22 | Y, 23 | Z, 24 | W 25 | } 26 | 27 | public double x; 28 | public double y; 29 | public double z; 30 | public double w; 31 | 32 | /// 33 | /// Useful for storing and retrieving Direction in DirMag Vector4s and Axis in AxisAngle Vector4s. 34 | /// 35 | public Vector3d XYZ 36 | { 37 | get 38 | { 39 | return new Vector3d(x, y, z); 40 | } 41 | set 42 | { 43 | x = value.x; 44 | y = value.y; 45 | z = value.z; 46 | } 47 | } 48 | 49 | public double this[int index] 50 | { 51 | get 52 | { 53 | switch (index) 54 | { 55 | case 0: 56 | return x; 57 | case 1: 58 | return y; 59 | case 2: 60 | return z; 61 | case 3: 62 | return w; 63 | default: 64 | throw new IndexOutOfRangeException(); 65 | } 66 | } 67 | set 68 | { 69 | switch (index) 70 | { 71 | case 0: 72 | x = value; 73 | return; 74 | case 1: 75 | y = value; 76 | return; 77 | case 2: 78 | z = value; 79 | return; 80 | case 3: 81 | w = value; 82 | return; 83 | default: 84 | throw new IndexOutOfRangeException(); 85 | } 86 | } 87 | } 88 | 89 | internal void Normalize() 90 | { 91 | double lengthsq = LengthSquared(); 92 | 93 | if (lengthsq == 0) 94 | { 95 | x = y = z = w = 0f; 96 | } 97 | else 98 | { 99 | double length = Mathd.Sqrt(lengthsq); 100 | x /= length; 101 | y /= length; 102 | z /= length; 103 | w /= length; 104 | } 105 | } 106 | 107 | public Vector4d Abs() 108 | { 109 | return new Vector4d(Mathd.Abs(x), Mathd.Abs(y), Mathd.Abs(z), Mathd.Abs(w)); 110 | } 111 | 112 | public Basisd AxisAngleBasis() 113 | { 114 | return new Basisd(XYZ, w); 115 | } 116 | 117 | public Quatd AxisAngleQuat() 118 | { 119 | return new Quatd(XYZ, w); 120 | } 121 | 122 | public static Vector4d AxisAngle(Quatd q) 123 | { 124 | double angle = 2 * Mathd.Acos(q.w); 125 | double den = Mathd.Sqrt(1 - q.w * q.w); 126 | if (den == 0) 127 | { 128 | return new Vector4d(0, 0, -1, angle); 129 | } 130 | return new Vector4d(q.x / den, q.y / den, q.z / den, angle); 131 | } 132 | 133 | public static Vector4d AxisAngle(Basisd b) 134 | { 135 | return AxisAngle(b.Quat()); // Might be a more efficient way to do this. 136 | } 137 | 138 | public Vector4d Bounce(Vector4d n) 139 | { 140 | return -Reflect(n); 141 | } 142 | 143 | public Vector4d Ceil() 144 | { 145 | return new Vector4d(Mathd.Ceil(x), Mathd.Ceil(y), Mathd.Ceil(z), Mathd.Ceil(w)); 146 | } 147 | 148 | public Vector4d CubicInterpolate(Vector4d b, Vector4d preA, Vector4d postB, double t) 149 | { 150 | var p0 = preA; 151 | var p1 = this; 152 | var p2 = b; 153 | var p3 = postB; 154 | 155 | double t2 = t * t; 156 | double t3 = t2 * t; 157 | 158 | return 0.5f * ( 159 | p1 * 2.0f + (-p0 + p2) * t + 160 | (2.0f * p0 - 5.0f * p1 + 4f * p2 - p3) * t2 + 161 | (-p0 + 3.0f * p1 - 3.0f * p2 + p3) * t3 162 | ); 163 | } 164 | 165 | public Vector4d DirectionTo(Vector4d b) 166 | { 167 | return new Vector4d(b.x - x, b.y - y, b.z - z, b.w - w).Normalized(); 168 | } 169 | 170 | public static Vector4d DirMag(Vector3d v) 171 | { 172 | return new Vector4d(v.Normalized(), v.Length()); 173 | } 174 | 175 | public Vector3d DirMag() 176 | { 177 | return w * XYZ; 178 | } 179 | 180 | public double DistanceSquaredTo(Vector4d b) 181 | { 182 | return (b - this).LengthSquared(); 183 | } 184 | 185 | public double DistanceTo(Vector4d b) 186 | { 187 | return (b - this).Length(); 188 | } 189 | 190 | public double Dot(Vector4d b) 191 | { 192 | return x * b.x + y * b.y + z * b.z + w * b.w; 193 | } 194 | 195 | public Vector4d Floor() 196 | { 197 | return new Vector4d(Mathd.Floor(x), Mathd.Floor(y), Mathd.Floor(z), Mathd.Floor(w)); 198 | } 199 | 200 | public Vector4d Inverse() 201 | { 202 | return new Vector4d(1.0f / x, 1.0f / y, 1.0f / z, 1.0f / w); 203 | } 204 | 205 | public bool IsNormalized() 206 | { 207 | return Mathd.Abs(LengthSquared() - 1.0f) < Mathd.Epsilon; 208 | } 209 | 210 | public double Length() 211 | { 212 | double x2 = x * x; 213 | double y2 = y * y; 214 | double z2 = z * z; 215 | double w2 = w * w; 216 | 217 | return Mathd.Sqrt(x2 + y2 + z2 + w2); 218 | } 219 | 220 | public double LengthSquared() 221 | { 222 | double x2 = x * x; 223 | double y2 = y * y; 224 | double z2 = z * z; 225 | double w2 = w * w; 226 | 227 | return x2 + y2 + z2 + w2; 228 | } 229 | 230 | /// 231 | /// Returns the result of the linear interpolation between 232 | /// this vector and `to` by amount `weight`. 233 | /// 234 | /// The destination vector for interpolation. 235 | /// A value on the range of 0.0 to 1.0, representing the amount of interpolation. 236 | /// The resulting vector of the interpolation. 237 | public Vector4d Lerp(Vector4d to, double weight) 238 | { 239 | return new Vector4d 240 | ( 241 | Mathd.Lerp(x, to.x, weight), 242 | Mathd.Lerp(y, to.y, weight), 243 | Mathd.Lerp(z, to.z, weight), 244 | Mathd.Lerp(w, to.w, weight) 245 | ); 246 | } 247 | 248 | /// 249 | /// Returns the result of the linear interpolation between 250 | /// this vector and `to` by the vector amount `weight`. 251 | /// 252 | /// The destination vector for interpolation. 253 | /// A vector with components on the range of 0.0 to 1.0, representing the amount of interpolation. 254 | /// The resulting vector of the interpolation. 255 | public Vector4d Lerp(Vector4d to, Vector4d weight) 256 | { 257 | return new Vector4d 258 | ( 259 | Mathd.Lerp(x, to.x, weight.x), 260 | Mathd.Lerp(y, to.y, weight.y), 261 | Mathd.Lerp(z, to.z, weight.z), 262 | Mathd.Lerp(w, to.w, weight.w) 263 | ); 264 | } 265 | 266 | /// 267 | /// Returns the result of the linear interpolation between 268 | /// this vector and `to` by amount `weight`. 269 | /// 270 | /// The destination vector for interpolation. 271 | /// A value on the range of 0.0 to 1.0, representing the amount of interpolation. 272 | /// The resulting vector of the interpolation. 273 | public Vector4d LinearInterpolate(Vector4d to, double weight) 274 | { 275 | return new Vector4d 276 | ( 277 | Mathd.Lerp(x, to.x, weight), 278 | Mathd.Lerp(y, to.y, weight), 279 | Mathd.Lerp(z, to.z, weight), 280 | Mathd.Lerp(w, to.w, weight) 281 | ); 282 | } 283 | 284 | /// 285 | /// Returns the result of the linear interpolation between 286 | /// this vector and `to` by the vector amount `weight`. 287 | /// 288 | /// The destination vector for interpolation. 289 | /// A vector with components on the range of 0.0 to 1.0, representing the amount of interpolation. 290 | /// The resulting vector of the interpolation. 291 | public Vector4d LinearInterpolate(Vector4d to, Vector4d weight) 292 | { 293 | return new Vector4d 294 | ( 295 | Mathd.Lerp(x, to.x, weight.x), 296 | Mathd.Lerp(y, to.y, weight.y), 297 | Mathd.Lerp(z, to.z, weight.z), 298 | Mathd.Lerp(w, to.w, weight.w) 299 | ); 300 | } 301 | 302 | public Axis MaxAxis() 303 | { 304 | byte index = 0; 305 | for (byte i = 1; i < 4; i++) 306 | { 307 | if (this[i] > this[index]) 308 | { 309 | index = i; 310 | } 311 | } 312 | return (Axis)index; 313 | } 314 | 315 | public Axis MinAxis() 316 | { 317 | byte index = 0; 318 | for (byte i = 1; i < 4; i++) 319 | { 320 | if (this[i] < this[index]) 321 | { 322 | index = i; 323 | } 324 | } 325 | return (Axis)index; 326 | } 327 | 328 | public Vector4d Normalized() 329 | { 330 | var v = this; 331 | v.Normalize(); 332 | return v; 333 | } 334 | 335 | public Vector4d PosMod(double mod) 336 | { 337 | Vector4d v = this; 338 | v.x = Mathd.PosMod(v.x, mod); 339 | v.y = Mathd.PosMod(v.y, mod); 340 | v.z = Mathd.PosMod(v.z, mod); 341 | v.w = Mathd.PosMod(v.w, mod); 342 | return v; 343 | } 344 | 345 | public Vector4d PosMod(Vector4d modv) 346 | { 347 | Vector4d v = this; 348 | v.x = Mathd.PosMod(v.x, modv.x); 349 | v.y = Mathd.PosMod(v.y, modv.y); 350 | v.z = Mathd.PosMod(v.z, modv.z); 351 | v.w = Mathd.PosMod(v.w, modv.w); 352 | return v; 353 | } 354 | 355 | public Vector4d Project(Vector4d onNormal) 356 | { 357 | return onNormal * (Dot(onNormal) / onNormal.LengthSquared()); 358 | } 359 | 360 | public Vector4d Reflect(Vector4d n) 361 | { 362 | #if DEBUG 363 | if (!n.IsNormalized()) 364 | throw new ArgumentException(String.Format("{0} is not normalized", n), nameof(n)); 365 | #endif 366 | return 2.0f * n * Dot(n) - this; 367 | } 368 | 369 | /// 370 | /// Rotates the given Basis, interpreting this Vector4 as AxisAngle. 371 | /// 372 | public Basisd Rotated(Basisd b) 373 | { 374 | return b * new Basisd(XYZ, w); 375 | } 376 | 377 | public Vector4d Round() 378 | { 379 | return new Vector4d(Mathd.Round(x), Mathd.Round(y), Mathd.Round(z), Mathd.Round(w)); 380 | } 381 | 382 | public Vector4d Sign() 383 | { 384 | Vector4d v = this; 385 | v.x = Mathd.Sign(v.x); 386 | v.y = Mathd.Sign(v.y); 387 | v.z = Mathd.Sign(v.z); 388 | v.w = Mathd.Sign(v.w); 389 | return v; 390 | } 391 | 392 | public Vector4d Slide(Vector4d n) 393 | { 394 | return this - n * Dot(n); 395 | } 396 | 397 | public Vector4d Snapped(Vector4d by) 398 | { 399 | return new Vector4d 400 | ( 401 | Mathd.Snapped(x, by.x), 402 | Mathd.Snapped(y, by.y), 403 | Mathd.Snapped(z, by.z), 404 | Mathd.Snapped(w, by.w) 405 | ); 406 | } 407 | 408 | public Vector2d[] UnpackVector2() 409 | { 410 | Vector2d[] arr = new Vector2d[2]; 411 | arr[0] = new Vector2d(x, y); 412 | arr[1] = new Vector2d(z, w); 413 | return arr; 414 | } 415 | 416 | public void UnpackVector2(out Vector2d xy, out Vector2d zw) 417 | { 418 | xy = new Vector2d(x, y); 419 | zw = new Vector2d(z, w); 420 | } 421 | 422 | // Constants 423 | private static readonly Vector4d _zero = new Vector4d(0, 0, 0, 0); 424 | private static readonly Vector4d _one = new Vector4d(1, 1, 1, 1); 425 | private static readonly Vector4d _negOne = new Vector4d(-1, -1, -1, -1); 426 | private static readonly Vector4d _inf = new Vector4d(Mathd.Inf, Mathd.Inf, Mathd.Inf, Mathd.Inf); 427 | 428 | private static readonly Vector4d _unitX = new Vector4d(1, 0, 0, 0); 429 | private static readonly Vector4d _unitY = new Vector4d(0, 1, 0, 0); 430 | private static readonly Vector4d _unitZ = new Vector4d(0, 0, 1, 0); 431 | private static readonly Vector4d _unitW = new Vector4d(0, 0, 0, 1); 432 | 433 | public static Vector4d Zero { get { return _zero; } } 434 | public static Vector4d One { get { return _one; } } 435 | public static Vector4d NegOne { get { return _negOne; } } 436 | public static Vector4d Inf { get { return _inf; } } 437 | 438 | public static Vector4d UnitX { get { return _unitX; } } 439 | public static Vector4d UnitY { get { return _unitY; } } 440 | public static Vector4d UnitZ { get { return _unitZ; } } 441 | public static Vector4d UnitW { get { return _unitW; } } 442 | 443 | // Constructors 444 | public Vector4d(double x, double y, double z, double w) 445 | { 446 | this.x = x; 447 | this.y = y; 448 | this.z = z; 449 | this.w = w; 450 | } 451 | public Vector4d(Vector4d v) 452 | { 453 | x = v.x; 454 | y = v.y; 455 | z = v.z; 456 | w = v.w; 457 | } 458 | public Vector4d(Vector3d xyz, double w) 459 | { 460 | x = xyz.x; 461 | y = xyz.y; 462 | z = xyz.z; 463 | this.w = w; 464 | } 465 | public Vector4d(Vector2d xy, Vector2d zw) 466 | { 467 | x = xy.x; 468 | y = xy.y; 469 | z = zw.x; 470 | w = zw.y; 471 | } 472 | 473 | public static explicit operator Vector4(Vector4d value) 474 | { 475 | return new Vector4((real_t)value.x, (real_t)value.y, (real_t)value.z, (real_t)value.w); 476 | } 477 | 478 | public static implicit operator Vector4d(Vector4 value) 479 | { 480 | return new Vector4d(value.x, value.y, value.z, value.w); 481 | } 482 | 483 | public static explicit operator Vector4i(Vector4d value) 484 | { 485 | return new Vector4i((int)value.x, (int)value.y, (int)value.z, (int)value.w); 486 | } 487 | 488 | public static implicit operator Vector4d(Vector4i value) 489 | { 490 | return new Vector4d(value.x, value.y, value.z, value.w); 491 | } 492 | 493 | #if UNITY_5_3_OR_NEWER 494 | public static explicit operator UnityEngine.Vector4(Vector4d value) 495 | { 496 | return new Vector4((real_t)value.x, (real_t)value.y, (real_t)value.z, (real_t)value.w); 497 | } 498 | 499 | public static implicit operator Vector4d(UnityEngine.Vector4 value) 500 | { 501 | return new Vector4d(value.x, value.y, value.z, value.w); 502 | } 503 | #endif 504 | 505 | public static Vector4d operator +(Vector4d left, Vector4d right) 506 | { 507 | left.x += right.x; 508 | left.y += right.y; 509 | left.z += right.z; 510 | left.w += right.w; 511 | return left; 512 | } 513 | 514 | public static Vector4d operator -(Vector4d left, Vector4d right) 515 | { 516 | left.x -= right.x; 517 | left.y -= right.y; 518 | left.z -= right.z; 519 | left.w -= right.w; 520 | return left; 521 | } 522 | 523 | public static Vector4d operator -(Vector4d vec) 524 | { 525 | vec.x = -vec.x; 526 | vec.y = -vec.y; 527 | vec.z = -vec.z; 528 | vec.w = -vec.w; 529 | return vec; 530 | } 531 | 532 | public static Vector4d operator *(Vector4d vec, double scale) 533 | { 534 | vec.x *= scale; 535 | vec.y *= scale; 536 | vec.z *= scale; 537 | vec.w *= scale; 538 | return vec; 539 | } 540 | 541 | public static Vector4d operator *(double scale, Vector4d vec) 542 | { 543 | vec.x *= scale; 544 | vec.y *= scale; 545 | vec.z *= scale; 546 | vec.w *= scale; 547 | return vec; 548 | } 549 | 550 | public static Vector4d operator *(Vector4d left, Vector4d right) 551 | { 552 | left.x *= right.x; 553 | left.y *= right.y; 554 | left.z *= right.z; 555 | left.w *= right.w; 556 | return left; 557 | } 558 | 559 | public static Vector4d operator /(Vector4d vec, double scale) 560 | { 561 | vec.x /= scale; 562 | vec.y /= scale; 563 | vec.z /= scale; 564 | vec.w /= scale; 565 | return vec; 566 | } 567 | 568 | public static Vector4d operator /(Vector4d left, Vector4d right) 569 | { 570 | left.x /= right.x; 571 | left.y /= right.y; 572 | left.z /= right.z; 573 | left.w /= right.w; 574 | return left; 575 | } 576 | 577 | public static Vector4d operator %(Vector4d vec, double divisor) 578 | { 579 | vec.x %= divisor; 580 | vec.y %= divisor; 581 | vec.z %= divisor; 582 | vec.w %= divisor; 583 | return vec; 584 | } 585 | 586 | public static Vector4d operator %(Vector4d vec, Vector4d divisorv) 587 | { 588 | vec.x %= divisorv.x; 589 | vec.y %= divisorv.y; 590 | vec.z %= divisorv.z; 591 | vec.w %= divisorv.w; 592 | return vec; 593 | } 594 | 595 | public static bool operator ==(Vector4d left, Vector4d right) 596 | { 597 | return left.Equals(right); 598 | } 599 | 600 | public static bool operator !=(Vector4d left, Vector4d right) 601 | { 602 | return !left.Equals(right); 603 | } 604 | 605 | public static bool operator <(Vector4d left, Vector4d right) 606 | { 607 | if (Mathd.IsEqualApprox(left.x, right.x)) 608 | { 609 | if (Mathd.IsEqualApprox(left.y, right.y)) 610 | { 611 | if (Mathd.IsEqualApprox(left.z, right.z)) 612 | { 613 | return left.w < right.w; 614 | } 615 | return left.z < right.z; 616 | } 617 | return left.y < right.y; 618 | } 619 | return left.x < right.x; 620 | } 621 | 622 | public static bool operator >(Vector4d left, Vector4d right) 623 | { 624 | if (Mathd.IsEqualApprox(left.x, right.x)) 625 | { 626 | if (Mathd.IsEqualApprox(left.y, right.y)) 627 | { 628 | if (Mathd.IsEqualApprox(left.z, right.z)) 629 | { 630 | return left.w > right.w; 631 | } 632 | return left.z > right.z; 633 | } 634 | return left.y > right.y; 635 | } 636 | return left.x > right.x; 637 | } 638 | 639 | public static bool operator <=(Vector4d left, Vector4d right) 640 | { 641 | if (Mathd.IsEqualApprox(left.x, right.x)) 642 | { 643 | if (Mathd.IsEqualApprox(left.y, right.y)) 644 | { 645 | if (Mathd.IsEqualApprox(left.z, right.z)) 646 | { 647 | return left.w <= right.w; 648 | } 649 | return left.z < right.z; 650 | } 651 | return left.y < right.y; 652 | } 653 | return left.x < right.x; 654 | } 655 | 656 | public static bool operator >=(Vector4d left, Vector4d right) 657 | { 658 | if (Mathd.IsEqualApprox(left.x, right.x)) 659 | { 660 | if (Mathd.IsEqualApprox(left.y, right.y)) 661 | { 662 | if (Mathd.IsEqualApprox(left.z, right.z)) 663 | { 664 | return left.w >= right.w; 665 | } 666 | return left.z > right.z; 667 | } 668 | return left.y > right.y; 669 | } 670 | return left.x > right.x; 671 | } 672 | 673 | public override bool Equals(object obj) 674 | { 675 | if (obj is Vector4d) 676 | { 677 | return Equals((Vector4d)obj); 678 | } 679 | 680 | return false; 681 | } 682 | 683 | public bool Equals(Vector4d other) 684 | { 685 | return Mathd.IsEqualApprox(x, other.x) && Mathd.IsEqualApprox(y, other.y) && Mathd.IsEqualApprox(z, other.z) && Mathd.IsEqualApprox(w, other.w); 686 | } 687 | 688 | public override int GetHashCode() 689 | { 690 | return y.GetHashCode() ^ x.GetHashCode() ^ z.GetHashCode() ^ w.GetHashCode(); 691 | } 692 | 693 | public override string ToString() 694 | { 695 | return String.Format("({0}, {1}, {2}, {3})", new object[] 696 | { 697 | x.ToString(), 698 | y.ToString(), 699 | z.ToString(), 700 | w.ToString() 701 | }); 702 | } 703 | 704 | public string ToString(string format) 705 | { 706 | return String.Format("({0}, {1}, {2}, {3})", new object[] 707 | { 708 | x.ToString(format), 709 | y.ToString(format), 710 | z.ToString(format), 711 | w.ToString(format) 712 | }); 713 | } 714 | } 715 | } 716 | -------------------------------------------------------------------------------- /addons/extra_math_cs/ExtraMath/Integer/Rect2i.cs: -------------------------------------------------------------------------------- 1 | #if GODOT 2 | using Godot; 3 | #elif UNITY_5_3_OR_NEWER 4 | using UnityEngine; 5 | #endif 6 | using System; 7 | using System.Runtime.InteropServices; 8 | 9 | namespace ExtraMath 10 | { 11 | /// 12 | /// 2D axis-aligned bounding box using integers. Rect2i consists of a position, a size, and 13 | /// several utility functions. It is typically used for fast overlap tests. 14 | /// 15 | [Serializable] 16 | [StructLayout(LayoutKind.Sequential)] 17 | public struct Rect2i : IEquatable 18 | { 19 | private Vector2i _position; 20 | private Vector2i _size; 21 | 22 | /// 23 | /// Beginning corner. Typically has values lower than End. 24 | /// 25 | /// Directly uses a private field. 26 | public Vector2i Position 27 | { 28 | get { return _position; } 29 | set { _position = value; } 30 | } 31 | 32 | /// 33 | /// Size from Position to End. Typically all components are positive. 34 | /// If the size is negative, you can use to fix it. 35 | /// 36 | /// Directly uses a private field. 37 | public Vector2i Size 38 | { 39 | get { return _size; } 40 | set { _size = value; } 41 | } 42 | 43 | /// 44 | /// Ending corner. This is calculated as plus 45 | /// . Setting this value will change the size. 46 | /// 47 | /// Getting is equivalent to `value = Position + Size`, setting is equivalent to `Size = value - Position`. 48 | public Vector2i End 49 | { 50 | get { return _position + _size; } 51 | set { _size = value - _position; } 52 | } 53 | 54 | /// 55 | /// The area of this Rect2i. 56 | /// 57 | /// Equivalent to . 58 | public int Area 59 | { 60 | get { return GetArea(); } 61 | } 62 | 63 | /// 64 | /// Returns a Rect2i with equivalent position and size, modified so that 65 | /// the top-left corner is the origin and width and height are positive. 66 | /// 67 | /// The modified Rect2i. 68 | public Rect2i Abs() 69 | { 70 | Vector2i end = End; 71 | Vector2i topLeft = new Vector2i(Mathf.Min(_position.x, end.x), Mathf.Min(_position.y, end.y)); 72 | return new Rect2i(topLeft, _size.Abs()); 73 | } 74 | 75 | /// 76 | /// Returns the intersection of this Rect2i and `b`. 77 | /// If the rectangles do not intersect, an empty Rect2i is returned. 78 | /// 79 | /// The other Rect2i. 80 | /// The intersection of this Rect2i and `b`, or an empty Rect2i if they do not intersect. 81 | public Rect2i Intersection(Rect2i b) 82 | { 83 | var newRect = b; 84 | 85 | if (!Intersects(newRect)) 86 | { 87 | return new Rect2i(); 88 | } 89 | 90 | newRect._position.x = Mathf.Max(b._position.x, _position.x); 91 | newRect._position.y = Mathf.Max(b._position.y, _position.y); 92 | 93 | Vector2i bEnd = b._position + b._size; 94 | Vector2i end = _position + _size; 95 | 96 | newRect._size.x = Mathf.Min(bEnd.x, end.x) - newRect._position.x; 97 | newRect._size.y = Mathf.Min(bEnd.y, end.y) - newRect._position.y; 98 | 99 | return newRect; 100 | } 101 | 102 | /// 103 | /// Returns true if this Rect2i completely encloses another one. 104 | /// 105 | /// The other Rect2i that may be enclosed. 106 | /// A bool for whether or not this Rect2i encloses `b`. 107 | public bool Encloses(Rect2i b) 108 | { 109 | return b._position.x >= _position.x && b._position.y >= _position.y && 110 | b._position.x + b._size.x < _position.x + _size.x && 111 | b._position.y + b._size.y < _position.y + _size.y; 112 | } 113 | 114 | /// 115 | /// Returns this Rect2i expanded to include a given point. 116 | /// 117 | /// The point to include. 118 | /// The expanded Rect2i. 119 | public Rect2i Expand(Vector2i to) 120 | { 121 | var expanded = this; 122 | 123 | Vector2i begin = expanded._position; 124 | Vector2i end = expanded._position + expanded._size; 125 | 126 | if (to.x < begin.x) 127 | { 128 | begin.x = to.x; 129 | } 130 | if (to.y < begin.y) 131 | { 132 | begin.y = to.y; 133 | } 134 | 135 | if (to.x > end.x) 136 | { 137 | end.x = to.x; 138 | } 139 | if (to.y > end.y) 140 | { 141 | end.y = to.y; 142 | } 143 | 144 | expanded._position = begin; 145 | expanded._size = end - begin; 146 | 147 | return expanded; 148 | } 149 | 150 | /// 151 | /// Returns the area of the Rect2. 152 | /// 153 | /// The area. 154 | public int GetArea() 155 | { 156 | return _size.x * _size.y; 157 | } 158 | 159 | /// 160 | /// Returns a copy of the Rect2i grown by the specified amount on all sides. 161 | /// 162 | /// The amount to grow by. 163 | /// The grown Rect2i. 164 | public Rect2i Grow(int by) 165 | { 166 | var g = this; 167 | 168 | g._position.x -= by; 169 | g._position.y -= by; 170 | g._size.x += 2 * by; 171 | g._size.y += 2 * by; 172 | 173 | return g; 174 | } 175 | 176 | /// 177 | /// Returns a copy of the Rect2i grown by the specified amount on each side individually. 178 | /// 179 | /// The amount to grow by on the left side. 180 | /// The amount to grow by on the top side. 181 | /// The amount to grow by on the right side. 182 | /// The amount to grow by on the bottom side. 183 | /// The grown Rect2i. 184 | public Rect2i GrowIndividual(int left, int top, int right, int bottom) 185 | { 186 | var g = this; 187 | 188 | g._position.x -= left; 189 | g._position.y -= top; 190 | g._size.x += left + right; 191 | g._size.y += top + bottom; 192 | 193 | return g; 194 | } 195 | 196 | #if GODOT 197 | /// 198 | /// Returns a copy of the Rect2i grown by the specified amount on the specified Side. 199 | /// 200 | /// The side to grow. 201 | /// The amount to grow by. 202 | /// The grown Rect2i. 203 | public Rect2i GrowMargin(Godot.Margin margin, int by) 204 | { 205 | var g = this; 206 | 207 | g = g.GrowIndividual(Godot.Margin.Left == margin ? by : 0, 208 | Godot.Margin.Top == margin ? by : 0, 209 | Godot.Margin.Right == margin ? by : 0, 210 | Godot.Margin.Bottom == margin ? by : 0); 211 | 212 | return g; 213 | } 214 | 215 | /// 216 | /// Returns a copy of the Rect2i grown by the specified amount on the specified Side. 217 | /// 218 | /// The side to grow. 219 | /// The amount to grow by. 220 | /// The grown Rect2i. 221 | public Rect2i GrowSide(Godot.Margin side, int by) 222 | { 223 | var g = this; 224 | 225 | g = g.GrowIndividual(Godot.Margin.Left == side ? by : 0, 226 | Godot.Margin.Top == side ? by : 0, 227 | Godot.Margin.Right == side ? by : 0, 228 | Godot.Margin.Bottom == side ? by : 0); 229 | 230 | return g; 231 | } 232 | #endif // GODOT 233 | 234 | /// 235 | /// Returns true if the Rect2i is flat or empty, or false otherwise. 236 | /// 237 | /// A bool for whether or not the Rect2i has area. 238 | public bool HasNoArea() 239 | { 240 | return _size.x <= 0 || _size.y <= 0; 241 | } 242 | 243 | /// 244 | /// Returns true if the Rect2i contains a point, or false otherwise. 245 | /// 246 | /// The point to check. 247 | /// A bool for whether or not the Rect2i contains `point`. 248 | public bool HasPoint(Vector2i point) 249 | { 250 | if (point.x < _position.x) 251 | return false; 252 | if (point.y < _position.y) 253 | return false; 254 | 255 | if (point.x >= _position.x + _size.x) 256 | return false; 257 | if (point.y >= _position.y + _size.y) 258 | return false; 259 | 260 | return true; 261 | } 262 | 263 | /// 264 | /// Returns true if the Rect2i overlaps with `b` 265 | /// (i.e. they have at least one point in common). 266 | /// 267 | /// If `includeBorders` is true, they will also be considered overlapping 268 | /// if their borders touch, even without intersection. 269 | /// 270 | /// The other Rect2i to check for intersections with. 271 | /// Whether or not to consider borders. 272 | /// A bool for whether or not they are intersecting. 273 | public bool Intersects(Rect2i b, bool includeBorders = false) 274 | { 275 | if (includeBorders) 276 | { 277 | if (_position.x > b._position.x + b._size.x) 278 | return false; 279 | if (_position.x + _size.x < b._position.x) 280 | return false; 281 | if (_position.y > b._position.y + b._size.y) 282 | return false; 283 | if (_position.y + _size.y < b._position.y) 284 | return false; 285 | } 286 | else 287 | { 288 | if (_position.x >= b._position.x + b._size.x) 289 | return false; 290 | if (_position.x + _size.x <= b._position.x) 291 | return false; 292 | if (_position.y >= b._position.y + b._size.y) 293 | return false; 294 | if (_position.y + _size.y <= b._position.y) 295 | return false; 296 | } 297 | 298 | return true; 299 | } 300 | 301 | /// 302 | /// Returns a larger Rect2i that contains this Rect2i and `b`. 303 | /// 304 | /// The other Rect2i. 305 | /// The merged Rect2i. 306 | public Rect2i Merge(Rect2i b) 307 | { 308 | Rect2i newRect; 309 | 310 | newRect._position.x = Mathf.Min(b._position.x, _position.x); 311 | newRect._position.y = Mathf.Min(b._position.y, _position.y); 312 | 313 | newRect._size.x = Mathf.Max(b._position.x + b._size.x, _position.x + _size.x); 314 | newRect._size.y = Mathf.Max(b._position.y + b._size.y, _position.y + _size.y); 315 | 316 | newRect._size -= newRect._position; // Make relative again 317 | 318 | return newRect; 319 | } 320 | 321 | /// 322 | /// Constructs a Rect2i from a position and size. 323 | /// 324 | /// The position. 325 | /// The size. 326 | public Rect2i(Vector2i position, Vector2i size) 327 | { 328 | _position = position; 329 | _size = size; 330 | } 331 | 332 | /// 333 | /// Constructs a Rect2i from a position, width, and height. 334 | /// 335 | /// The position. 336 | /// The width. 337 | /// The height. 338 | public Rect2i(Vector2i position, int width, int height) 339 | { 340 | _position = position; 341 | _size = new Vector2i(width, height); 342 | } 343 | 344 | /// 345 | /// Constructs a Rect2i from x, y, and size. 346 | /// 347 | /// The position's X coordinate. 348 | /// The position's Y coordinate. 349 | /// The size. 350 | public Rect2i(int x, int y, Vector2i size) 351 | { 352 | _position = new Vector2i(x, y); 353 | _size = size; 354 | } 355 | 356 | /// 357 | /// Constructs a Rect2i from x, y, width, and height. 358 | /// 359 | /// The position's X coordinate. 360 | /// The position's Y coordinate. 361 | /// The width. 362 | /// The height. 363 | public Rect2i(int x, int y, int width, int height) 364 | { 365 | _position = new Vector2i(x, y); 366 | _size = new Vector2i(width, height); 367 | } 368 | 369 | public static bool operator ==(Rect2i left, Rect2i right) 370 | { 371 | return left.Equals(right); 372 | } 373 | 374 | public static bool operator !=(Rect2i left, Rect2i right) 375 | { 376 | return !left.Equals(right); 377 | } 378 | 379 | #if GODOT 380 | public static explicit operator Rect2i(Godot.Rect2 value) 381 | { 382 | return new Rect2i((Vector2i)value.Position, (Vector2i)value.Size); 383 | } 384 | 385 | public static implicit operator Godot.Rect2(Rect2i value) 386 | { 387 | return new Godot.Rect2(value.Position, value.Size); 388 | } 389 | #endif 390 | 391 | public override bool Equals(object obj) 392 | { 393 | if (obj is Rect2i) 394 | { 395 | return Equals((Rect2i)obj); 396 | } 397 | 398 | return false; 399 | } 400 | 401 | public bool Equals(Rect2i other) 402 | { 403 | return _position.Equals(other._position) && _size.Equals(other._size); 404 | } 405 | 406 | public override int GetHashCode() 407 | { 408 | return _position.GetHashCode() ^ _size.GetHashCode(); 409 | } 410 | 411 | public override string ToString() 412 | { 413 | return String.Format("{0}, {1}", new object[] 414 | { 415 | _position.ToString(), 416 | _size.ToString() 417 | }); 418 | } 419 | 420 | public string ToString(string format) 421 | { 422 | return String.Format("{0}, {1}", new object[] 423 | { 424 | _position.ToString(format), 425 | _size.ToString(format) 426 | }); 427 | } 428 | } 429 | } 430 | -------------------------------------------------------------------------------- /addons/extra_math_cs/ExtraMath/Integer/Vector2i.cs: -------------------------------------------------------------------------------- 1 | #if GODOT 2 | using Godot; 3 | #elif UNITY_5_3_OR_NEWER 4 | using UnityEngine; 5 | #endif 6 | using System; 7 | using System.Runtime.InteropServices; 8 | 9 | #if GODOT_REAL_T_IS_DOUBLE 10 | using real_t = System.Double; 11 | #else 12 | using real_t = System.Single; 13 | #endif 14 | 15 | namespace ExtraMath 16 | { 17 | /// 18 | /// 2-element structure that can be used to represent 2D grid coordinates or pairs of integers. 19 | /// 20 | [Serializable] 21 | [StructLayout(LayoutKind.Sequential)] 22 | public struct Vector2i : IEquatable 23 | { 24 | /// 25 | /// Enumerated index values for the axes. 26 | /// Returned by and . 27 | /// 28 | public enum Axis 29 | { 30 | X = 0, 31 | Y 32 | } 33 | 34 | /// 35 | /// The vector's X component. Also accessible by using the index position `[0]`. 36 | /// 37 | public int x; 38 | /// 39 | /// The vector's Y component. Also accessible by using the index position `[1]`. 40 | /// 41 | public int y; 42 | 43 | /// 44 | /// Access vector components using their index. 45 | /// 46 | /// `[0]` is equivalent to `.x`, `[1]` is equivalent to `.y`. 47 | public int this[int index] 48 | { 49 | get 50 | { 51 | switch (index) 52 | { 53 | case 0: 54 | return x; 55 | case 1: 56 | return y; 57 | default: 58 | throw new IndexOutOfRangeException(); 59 | } 60 | } 61 | set 62 | { 63 | switch (index) 64 | { 65 | case 0: 66 | x = value; 67 | return; 68 | case 1: 69 | y = value; 70 | return; 71 | default: 72 | throw new IndexOutOfRangeException(); 73 | } 74 | } 75 | } 76 | 77 | /// 78 | /// Returns a new vector with all components in absolute values (i.e. positive). 79 | /// 80 | /// A vector with called on each component. 81 | public Vector2i Abs() 82 | { 83 | return new Vector2i(Mathf.Abs(x), Mathf.Abs(y)); 84 | } 85 | 86 | /// 87 | /// Returns this vector's angle with respect to the X axis, or (1, 0) vector, in radians. 88 | /// 89 | /// Equivalent to the result of when 90 | /// called with the vector's `y` and `x` as parameters: `Mathf.Atan2(v.y, v.x)`. 91 | /// 92 | /// The angle of this vector, in radians. 93 | public real_t Angle() 94 | { 95 | return Mathf.Atan2(y, x); 96 | } 97 | 98 | /// 99 | /// Returns the angle to the given vector, in radians. 100 | /// 101 | /// The other vector to compare this vector to. 102 | /// The angle between the two vectors, in radians. 103 | public real_t AngleTo(Vector2i to) 104 | { 105 | return Mathf.Atan2(Cross(to), Dot(to)); 106 | } 107 | 108 | /// 109 | /// Returns the angle between the line connecting the two points and the X axis, in radians. 110 | /// 111 | /// The other vector to compare this vector to. 112 | /// The angle between the two vectors, in radians. 113 | public real_t AngleToPoint(Vector2i to) 114 | { 115 | return Mathf.Atan2(y - to.y, x - to.x); 116 | } 117 | 118 | /// 119 | /// Returns the aspect ratio of this vector, the ratio of `x` to `y`. 120 | /// 121 | /// The `x` component divided by the `y` component. 122 | public real_t Aspect() 123 | { 124 | return x / (real_t)y; 125 | } 126 | 127 | /// 128 | /// Returns the cross product of this vector and `b`. 129 | /// 130 | /// The other vector. 131 | /// The cross product vector. 132 | public int Cross(Vector2i b) 133 | { 134 | return x * b.y - y * b.x; 135 | } 136 | 137 | /// 138 | /// Returns the squared distance between this vector and `b`. 139 | /// This method runs faster than , so prefer it if 140 | /// you need to compare vectors or need the squared distance for some formula. 141 | /// 142 | /// The other vector to use. 143 | /// The squared distance between the two vectors. 144 | public int DistanceSquaredTo(Vector2i b) 145 | { 146 | return (b - this).LengthSquared(); 147 | } 148 | 149 | /// 150 | /// Returns the distance between this vector and `b`. 151 | /// 152 | /// The other vector to use. 153 | /// The distance between the two vectors. 154 | public real_t DistanceTo(Vector2i b) 155 | { 156 | return (b - this).Length(); 157 | } 158 | 159 | /// 160 | /// Returns the dot product of this vector and `b`. 161 | /// 162 | /// The other vector to use. 163 | /// The dot product of the two vectors. 164 | public int Dot(Vector2i b) 165 | { 166 | return x * b.x + y * b.y; 167 | } 168 | 169 | /// 170 | /// Returns the length (magnitude) of this vector. 171 | /// 172 | /// The length of this vector. 173 | public real_t Length() 174 | { 175 | int x2 = x * x; 176 | int y2 = y * y; 177 | 178 | return Mathf.Sqrt(x2 + y2); 179 | } 180 | 181 | /// 182 | /// Returns the squared length (squared magnitude) of this vector. 183 | /// This method runs faster than , so prefer it if 184 | /// you need to compare vectors or need the squared length for some formula. 185 | /// 186 | /// The squared length of this vector. 187 | public int LengthSquared() 188 | { 189 | int x2 = x * x; 190 | int y2 = y * y; 191 | 192 | return x2 + y2; 193 | } 194 | 195 | /// 196 | /// Returns the axis of the vector's largest value. See . 197 | /// If both components are equal, this method returns . 198 | /// 199 | /// The index of the largest axis. 200 | public Axis MaxAxis() 201 | { 202 | return x < y ? Axis.Y : Axis.X; 203 | } 204 | 205 | /// 206 | /// Returns the axis of the vector's smallest value. See . 207 | /// If both components are equal, this method returns . 208 | /// 209 | /// The index of the smallest axis. 210 | public Axis MinAxis() 211 | { 212 | return x < y ? Axis.X : Axis.Y; 213 | } 214 | 215 | #if GODOT 216 | /// 217 | /// Returns a vector composed of the of this vector's components and `mod`. 218 | /// 219 | /// A value representing the divisor of the operation. 220 | /// A vector with each component by `mod`. 221 | public Vector2i PosMod(int mod) 222 | { 223 | Vector2i v = this; 224 | v.x = Mathf.PosMod(v.x, mod); 225 | v.y = Mathf.PosMod(v.y, mod); 226 | return v; 227 | } 228 | 229 | /// 230 | /// Returns a vector composed of the of this vector's components and `modv`'s components. 231 | /// 232 | /// A vector representing the divisors of the operation. 233 | /// A vector with each component by `modv`'s components. 234 | public Vector2i PosMod(Vector2i modv) 235 | { 236 | Vector2i v = this; 237 | v.x = Mathf.PosMod(v.x, modv.x); 238 | v.y = Mathf.PosMod(v.y, modv.y); 239 | return v; 240 | } 241 | #endif // GODOT 242 | 243 | /// 244 | /// Returns a vector with each component set to one or negative one, depending 245 | /// on the signs of this vector's components, or zero if the component is zero, 246 | /// by calling on each component. 247 | /// 248 | /// A vector with all components as either `1`, `-1`, or `0`. 249 | public Vector2i Sign() 250 | { 251 | Vector2i v = this; 252 | #if GODOT 253 | v.x = Mathf.Sign(v.x); 254 | v.y = Mathf.Sign(v.y); 255 | #else 256 | v.x = v.x < 0 ? -1 : 1; 257 | v.y = v.y < 0 ? -1 : 1; 258 | #endif 259 | return v; 260 | } 261 | 262 | /// 263 | /// Returns a perpendicular vector rotated 90 degrees counter-clockwise 264 | /// compared to the original, with the same length. 265 | /// 266 | /// The perpendicular vector. 267 | public Vector2i Orthogonal() 268 | { 269 | return new Vector2i(y, -x); 270 | } 271 | 272 | // Constants 273 | private static readonly Vector2i _zero = new Vector2i(0, 0); 274 | private static readonly Vector2i _one = new Vector2i(1, 1); 275 | 276 | private static readonly Vector2i _up = new Vector2i(0, -1); 277 | private static readonly Vector2i _down = new Vector2i(0, 1); 278 | private static readonly Vector2i _right = new Vector2i(1, 0); 279 | private static readonly Vector2i _left = new Vector2i(-1, 0); 280 | 281 | /// 282 | /// Zero vector, a vector with all components set to `0`. 283 | /// 284 | /// Equivalent to `new Vector2i(0, 0)` 285 | public static Vector2i Zero { get { return _zero; } } 286 | /// 287 | /// One vector, a vector with all components set to `1`. 288 | /// 289 | /// Equivalent to `new Vector2i(1, 1)` 290 | public static Vector2i One { get { return _one; } } 291 | 292 | /// 293 | /// Up unit vector. Y is down in 2D, so this vector points -Y. 294 | /// 295 | /// Equivalent to `new Vector2i(0, -1)` 296 | public static Vector2i Up { get { return _up; } } 297 | /// 298 | /// Down unit vector. Y is down in 2D, so this vector points +Y. 299 | /// 300 | /// Equivalent to `new Vector2i(0, 1)` 301 | public static Vector2i Down { get { return _down; } } 302 | /// 303 | /// Right unit vector. Represents the direction of right. 304 | /// 305 | /// Equivalent to `new Vector2i(1, 0)` 306 | public static Vector2i Right { get { return _right; } } 307 | /// 308 | /// Left unit vector. Represents the direction of left. 309 | /// 310 | /// Equivalent to `new Vector2i(-1, 0)` 311 | public static Vector2i Left { get { return _left; } } 312 | 313 | /// 314 | /// Constructs a new with the given components. 315 | /// 316 | /// The vector's X component. 317 | /// The vector's Y component. 318 | public Vector2i(int x, int y) 319 | { 320 | this.x = x; 321 | this.y = y; 322 | } 323 | 324 | /// 325 | /// Constructs a new from an existing . 326 | /// 327 | /// The existing . 328 | public Vector2i(Vector2i vi) 329 | { 330 | this.x = vi.x; 331 | this.y = vi.y; 332 | } 333 | 334 | /// 335 | /// Constructs a new from an existing 336 | /// by rounding the components via . 337 | /// 338 | /// The to convert. 339 | public Vector2i(Vector2 v) 340 | { 341 | this.x = Mathf.RoundToInt(v.x); 342 | this.y = Mathf.RoundToInt(v.y); 343 | } 344 | 345 | public static Vector2i operator +(Vector2i left, Vector2i right) 346 | { 347 | left.x += right.x; 348 | left.y += right.y; 349 | return left; 350 | } 351 | 352 | public static Vector2i operator -(Vector2i left, Vector2i right) 353 | { 354 | left.x -= right.x; 355 | left.y -= right.y; 356 | return left; 357 | } 358 | 359 | public static Vector2i operator -(Vector2i vec) 360 | { 361 | vec.x = -vec.x; 362 | vec.y = -vec.y; 363 | return vec; 364 | } 365 | 366 | public static Vector2i operator *(Vector2i vec, int scale) 367 | { 368 | vec.x *= scale; 369 | vec.y *= scale; 370 | return vec; 371 | } 372 | 373 | public static Vector2i operator *(int scale, Vector2i vec) 374 | { 375 | vec.x *= scale; 376 | vec.y *= scale; 377 | return vec; 378 | } 379 | 380 | public static Vector2i operator *(Vector2i left, Vector2i right) 381 | { 382 | left.x *= right.x; 383 | left.y *= right.y; 384 | return left; 385 | } 386 | 387 | public static Vector2i operator /(Vector2i vec, int divisor) 388 | { 389 | vec.x /= divisor; 390 | vec.y /= divisor; 391 | return vec; 392 | } 393 | 394 | public static Vector2i operator /(Vector2i vec, Vector2i divisorv) 395 | { 396 | vec.x /= divisorv.x; 397 | vec.y /= divisorv.y; 398 | return vec; 399 | } 400 | 401 | public static Vector2i operator %(Vector2i vec, int divisor) 402 | { 403 | vec.x %= divisor; 404 | vec.y %= divisor; 405 | return vec; 406 | } 407 | 408 | public static Vector2i operator %(Vector2i vec, Vector2i divisorv) 409 | { 410 | vec.x %= divisorv.x; 411 | vec.y %= divisorv.y; 412 | return vec; 413 | } 414 | 415 | public static Vector2i operator &(Vector2i vec, int and) 416 | { 417 | vec.x &= and; 418 | vec.y &= and; 419 | return vec; 420 | } 421 | 422 | public static Vector2i operator &(Vector2i vec, Vector2i andv) 423 | { 424 | vec.x &= andv.x; 425 | vec.y &= andv.y; 426 | return vec; 427 | } 428 | 429 | public static bool operator ==(Vector2i left, Vector2i right) 430 | { 431 | return left.Equals(right); 432 | } 433 | 434 | public static bool operator !=(Vector2i left, Vector2i right) 435 | { 436 | return !left.Equals(right); 437 | } 438 | 439 | public static bool operator <(Vector2i left, Vector2i right) 440 | { 441 | if (left.x.Equals(right.x)) 442 | { 443 | return left.y < right.y; 444 | } 445 | return left.x < right.x; 446 | } 447 | 448 | public static bool operator >(Vector2i left, Vector2i right) 449 | { 450 | if (left.x.Equals(right.x)) 451 | { 452 | return left.y > right.y; 453 | } 454 | return left.x > right.x; 455 | } 456 | 457 | public static bool operator <=(Vector2i left, Vector2i right) 458 | { 459 | if (left.x.Equals(right.x)) 460 | { 461 | return left.y <= right.y; 462 | } 463 | return left.x <= right.x; 464 | } 465 | 466 | public static bool operator >=(Vector2i left, Vector2i right) 467 | { 468 | if (left.x.Equals(right.x)) 469 | { 470 | return left.y >= right.y; 471 | } 472 | return left.x >= right.x; 473 | } 474 | 475 | public static implicit operator Vector2(Vector2i value) 476 | { 477 | return new Vector2(value.x, value.y); 478 | } 479 | 480 | public static explicit operator Vector2i(Vector2 value) 481 | { 482 | return new Vector2i(value); 483 | } 484 | 485 | #if UNITY_5_3_OR_NEWER 486 | public static implicit operator Vector2Int(Vector2i value) 487 | { 488 | return new Vector2Int(value.x, value.y); 489 | } 490 | 491 | public static explicit operator Vector2i(Vector2Int value) 492 | { 493 | return new Vector2i(value); 494 | } 495 | #endif 496 | 497 | public override bool Equals(object obj) 498 | { 499 | if (obj is Vector2i) 500 | { 501 | return Equals((Vector2i)obj); 502 | } 503 | 504 | return false; 505 | } 506 | 507 | public bool Equals(Vector2i other) 508 | { 509 | return x == other.x && y == other.y; 510 | } 511 | 512 | public override int GetHashCode() 513 | { 514 | return y.GetHashCode() ^ x.GetHashCode(); 515 | } 516 | 517 | public override string ToString() 518 | { 519 | return String.Format("({0}, {1})", new object[] 520 | { 521 | this.x.ToString(), 522 | this.y.ToString() 523 | }); 524 | } 525 | 526 | public string ToString(string format) 527 | { 528 | return String.Format("({0}, {1})", new object[] 529 | { 530 | this.x.ToString(format), 531 | this.y.ToString(format) 532 | }); 533 | } 534 | } 535 | } 536 | -------------------------------------------------------------------------------- /addons/extra_math_cs/ExtraMath/Integer/Vector3i.cs: -------------------------------------------------------------------------------- 1 | #if GODOT 2 | using Godot; 3 | #elif UNITY_5_3_OR_NEWER 4 | using UnityEngine; 5 | #endif 6 | using System; 7 | using System.Runtime.InteropServices; 8 | 9 | #if GODOT_REAL_T_IS_DOUBLE 10 | using real_t = System.Double; 11 | #else 12 | using real_t = System.Single; 13 | #endif 14 | 15 | namespace ExtraMath 16 | { 17 | /// 18 | /// 3-element structure that can be used to represent 3D grid coordinates or sets of integers. 19 | /// 20 | [Serializable] 21 | [StructLayout(LayoutKind.Sequential)] 22 | public struct Vector3i : IEquatable 23 | { 24 | /// 25 | /// Enumerated index values for the axes. 26 | /// Returned by and . 27 | /// 28 | public enum Axis 29 | { 30 | X = 0, 31 | Y, 32 | Z 33 | } 34 | 35 | /// 36 | /// The vector's X component. Also accessible by using the index position `[0]`. 37 | /// 38 | public int x; 39 | /// 40 | /// The vector's Y component. Also accessible by using the index position `[1]`. 41 | /// 42 | public int y; 43 | /// 44 | /// The vector's Z component. Also accessible by using the index position `[2]`. 45 | /// 46 | public int z; 47 | 48 | /// 49 | /// Access vector components using their index. 50 | /// 51 | /// `[0]` is equivalent to `.x`, `[1]` is equivalent to `.y`, `[2]` is equivalent to `.z`. 52 | public int this[int index] 53 | { 54 | get 55 | { 56 | switch (index) 57 | { 58 | case 0: 59 | return x; 60 | case 1: 61 | return y; 62 | case 2: 63 | return z; 64 | default: 65 | throw new IndexOutOfRangeException(); 66 | } 67 | } 68 | set 69 | { 70 | switch (index) 71 | { 72 | case 0: 73 | x = value; 74 | return; 75 | case 1: 76 | y = value; 77 | return; 78 | case 2: 79 | z = value; 80 | return; 81 | default: 82 | throw new IndexOutOfRangeException(); 83 | } 84 | } 85 | } 86 | 87 | /// 88 | /// Returns a new vector with all components in absolute values (i.e. positive). 89 | /// 90 | /// A vector with called on each component. 91 | public Vector3i Abs() 92 | { 93 | return new Vector3i(Mathf.Abs(x), Mathf.Abs(y), Mathf.Abs(z)); 94 | } 95 | 96 | /// 97 | /// Returns the cross product of this vector and `b`. 98 | /// 99 | /// The other vector. 100 | /// The cross product vector. 101 | public Vector3i Cross(Vector3i b) 102 | { 103 | return new Vector3i 104 | ( 105 | y * b.z - z * b.y, 106 | z * b.x - x * b.z, 107 | x * b.y - y * b.x 108 | ); 109 | } 110 | 111 | /// 112 | /// Returns the squared distance between this vector and `b`. 113 | /// This method runs faster than , so prefer it if 114 | /// you need to compare vectors or need the squared distance for some formula. 115 | /// 116 | /// The other vector to use. 117 | /// The squared distance between the two vectors. 118 | public int DistanceSquaredTo(Vector3i b) 119 | { 120 | return (b - this).LengthSquared(); 121 | } 122 | 123 | /// 124 | /// Returns the distance between this vector and `b`. 125 | /// 126 | /// The other vector to use. 127 | /// The distance between the two vectors. 128 | public real_t DistanceTo(Vector3i b) 129 | { 130 | return (b - this).Length(); 131 | } 132 | 133 | /// 134 | /// Returns the dot product of this vector and `b`. 135 | /// 136 | /// The other vector to use. 137 | /// The dot product of the two vectors. 138 | public int Dot(Vector3i b) 139 | { 140 | return x * b.x + y * b.y + z * b.z; 141 | } 142 | 143 | /// 144 | /// Returns the length (magnitude) of this vector. 145 | /// 146 | /// The length of this vector. 147 | public real_t Length() 148 | { 149 | int x2 = x * x; 150 | int y2 = y * y; 151 | int z2 = z * z; 152 | 153 | return Mathf.Sqrt(x2 + y2 + z2); 154 | } 155 | 156 | /// 157 | /// Returns the squared length (squared magnitude) of this vector. 158 | /// This method runs faster than , so prefer it if 159 | /// you need to compare vectors or need the squared length for some formula. 160 | /// 161 | /// The squared length of this vector. 162 | public int LengthSquared() 163 | { 164 | int x2 = x * x; 165 | int y2 = y * y; 166 | int z2 = z * z; 167 | 168 | return x2 + y2 + z2; 169 | } 170 | 171 | /// 172 | /// Returns the axis of the vector's largest value. See . 173 | /// If all components are equal, this method returns . 174 | /// 175 | /// The index of the largest axis. 176 | public Axis MaxAxis() 177 | { 178 | return x < y ? (y < z ? Axis.Z : Axis.Y) : (x < z ? Axis.Z : Axis.X); 179 | } 180 | 181 | /// 182 | /// Returns the axis of the vector's smallest value. See . 183 | /// If all components are equal, this method returns . 184 | /// 185 | /// The index of the smallest axis. 186 | public Axis MinAxis() 187 | { 188 | return x < y ? (x < z ? Axis.X : Axis.Z) : (y < z ? Axis.Y : Axis.Z); 189 | } 190 | 191 | #if GODOT 192 | /// 193 | /// Returns a vector composed of the of this vector's components and `mod`. 194 | /// 195 | /// A value representing the divisor of the operation. 196 | /// A vector with each component by `mod`. 197 | public Vector3i PosMod(int mod) 198 | { 199 | Vector3i v = this; 200 | v.x = Mathf.PosMod(v.x, mod); 201 | v.y = Mathf.PosMod(v.y, mod); 202 | v.z = Mathf.PosMod(v.z, mod); 203 | return v; 204 | } 205 | 206 | /// 207 | /// Returns a vector composed of the of this vector's components and `modv`'s components. 208 | /// 209 | /// A vector representing the divisors of the operation. 210 | /// A vector with each component by `modv`'s components. 211 | public Vector3i PosMod(Vector3i modv) 212 | { 213 | Vector3i v = this; 214 | v.x = Mathf.PosMod(v.x, modv.x); 215 | v.y = Mathf.PosMod(v.y, modv.y); 216 | v.z = Mathf.PosMod(v.z, modv.z); 217 | return v; 218 | } 219 | #endif 220 | 221 | /// 222 | /// Returns a vector with each component set to one or negative one, depending 223 | /// on the signs of this vector's components, or zero if the component is zero, 224 | /// by calling on each component. 225 | /// 226 | /// A vector with all components as either `1`, `-1`, or `0`. 227 | public Vector3i Sign() 228 | { 229 | Vector3i v = this; 230 | #if GODOT 231 | v.x = Mathf.Sign(v.x); 232 | v.y = Mathf.Sign(v.y); 233 | v.z = Mathf.Sign(v.z); 234 | #else 235 | v.x = v.x < 0 ? -1 : 1; 236 | v.y = v.y < 0 ? -1 : 1; 237 | v.z = v.z < 0 ? -1 : 1; 238 | #endif 239 | return v; 240 | } 241 | 242 | /// 243 | /// Returns the signed angle to the given vector, in radians. 244 | /// The sign of the angle is positive in a counter-clockwise 245 | /// direction and negative in a clockwise direction when viewed 246 | /// from the side specified by the `axis`. 247 | /// 248 | /// The other vector to compare this vector to. 249 | /// The reference axis to use for the angle sign. 250 | /// The signed angle between the two vectors, in radians. 251 | public real_t SignedAngleTo(Vector3i to, Vector3i axis) 252 | { 253 | Vector3i crossTo = Cross(to); 254 | real_t unsignedAngle = Mathf.Atan2(crossTo.Length(), Dot(to)); 255 | real_t sign = crossTo.Dot(axis); 256 | return (sign < 0) ? -unsignedAngle : unsignedAngle; 257 | } 258 | 259 | // Constants 260 | private static readonly Vector3i _zero = new Vector3i(0, 0, 0); 261 | private static readonly Vector3i _one = new Vector3i(1, 1, 1); 262 | 263 | private static readonly Vector3i _up = new Vector3i(0, 1, 0); 264 | private static readonly Vector3i _down = new Vector3i(0, -1, 0); 265 | private static readonly Vector3i _right = new Vector3i(1, 0, 0); 266 | private static readonly Vector3i _left = new Vector3i(-1, 0, 0); 267 | private static readonly Vector3i _forward = new Vector3i(0, 0, -1); 268 | private static readonly Vector3i _back = new Vector3i(0, 0, 1); 269 | 270 | /// 271 | /// Zero vector, a vector with all components set to `0`. 272 | /// 273 | /// Equivalent to `new Vector3i(0, 0, 0)` 274 | public static Vector3i Zero { get { return _zero; } } 275 | /// 276 | /// One vector, a vector with all components set to `1`. 277 | /// 278 | /// Equivalent to `new Vector3i(1, 1, 1)` 279 | public static Vector3i One { get { return _one; } } 280 | 281 | /// 282 | /// Up unit vector. 283 | /// 284 | /// Equivalent to `new Vector3i(0, 1, 0)` 285 | public static Vector3i Up { get { return _up; } } 286 | /// 287 | /// Down unit vector. 288 | /// 289 | /// Equivalent to `new Vector3i(0, -1, 0)` 290 | public static Vector3i Down { get { return _down; } } 291 | /// 292 | /// Right unit vector. Represents the local direction of right, 293 | /// and the global direction of east. 294 | /// 295 | /// Equivalent to `new Vector3i(1, 0, 0)` 296 | public static Vector3i Right { get { return _right; } } 297 | /// 298 | /// Left unit vector. Represents the local direction of left, 299 | /// and the global direction of west. 300 | /// 301 | /// Equivalent to `new Vector3i(-1, 0, 0)` 302 | public static Vector3i Left { get { return _left; } } 303 | /// 304 | /// Forward unit vector. Represents the local direction of forward, 305 | /// and the global direction of north. 306 | /// 307 | /// Equivalent to `new Vector3i(0, 0, -1)` 308 | public static Vector3i Forward { get { return _forward; } } 309 | /// 310 | /// Back unit vector. Represents the local direction of back, 311 | /// and the global direction of south. 312 | /// 313 | /// Equivalent to `new Vector3i(0, 0, 1)` 314 | public static Vector3i Back { get { return _back; } } 315 | 316 | /// 317 | /// Constructs a new with the given components. 318 | /// 319 | /// The vector's X component. 320 | /// The vector's Y component. 321 | /// The vector's Z component. 322 | public Vector3i(int x, int y, int z) 323 | { 324 | this.x = x; 325 | this.y = y; 326 | this.z = z; 327 | } 328 | 329 | /// 330 | /// Constructs a new from an existing . 331 | /// 332 | /// The existing . 333 | public Vector3i(Vector3i vi) 334 | { 335 | this.x = vi.x; 336 | this.y = vi.y; 337 | this.z = vi.z; 338 | } 339 | 340 | /// 341 | /// Constructs a new from an existing 342 | /// by rounding the components via . 343 | /// 344 | /// The to convert. 345 | public Vector3i(Vector3 v) 346 | { 347 | this.x = Mathf.RoundToInt(v.x); 348 | this.y = Mathf.RoundToInt(v.y); 349 | this.z = Mathf.RoundToInt(v.z); 350 | } 351 | 352 | public static Vector3i operator +(Vector3i left, Vector3i right) 353 | { 354 | left.x += right.x; 355 | left.y += right.y; 356 | left.z += right.z; 357 | return left; 358 | } 359 | 360 | public static Vector3i operator -(Vector3i left, Vector3i right) 361 | { 362 | left.x -= right.x; 363 | left.y -= right.y; 364 | left.z -= right.z; 365 | return left; 366 | } 367 | 368 | public static Vector3i operator -(Vector3i vec) 369 | { 370 | vec.x = -vec.x; 371 | vec.y = -vec.y; 372 | vec.z = -vec.z; 373 | return vec; 374 | } 375 | 376 | public static Vector3i operator *(Vector3i vec, int scale) 377 | { 378 | vec.x *= scale; 379 | vec.y *= scale; 380 | vec.z *= scale; 381 | return vec; 382 | } 383 | 384 | public static Vector3i operator *(int scale, Vector3i vec) 385 | { 386 | vec.x *= scale; 387 | vec.y *= scale; 388 | vec.z *= scale; 389 | return vec; 390 | } 391 | 392 | public static Vector3i operator *(Vector3i left, Vector3i right) 393 | { 394 | left.x *= right.x; 395 | left.y *= right.y; 396 | left.z *= right.z; 397 | return left; 398 | } 399 | 400 | public static Vector3i operator /(Vector3i vec, int divisor) 401 | { 402 | vec.x /= divisor; 403 | vec.y /= divisor; 404 | vec.z /= divisor; 405 | return vec; 406 | } 407 | 408 | public static Vector3i operator /(Vector3i vec, Vector3i divisorv) 409 | { 410 | vec.x /= divisorv.x; 411 | vec.y /= divisorv.y; 412 | vec.z /= divisorv.z; 413 | return vec; 414 | } 415 | 416 | public static Vector3i operator %(Vector3i vec, int divisor) 417 | { 418 | vec.x %= divisor; 419 | vec.y %= divisor; 420 | vec.z %= divisor; 421 | return vec; 422 | } 423 | 424 | public static Vector3i operator %(Vector3i vec, Vector3i divisorv) 425 | { 426 | vec.x %= divisorv.x; 427 | vec.y %= divisorv.y; 428 | vec.z %= divisorv.z; 429 | return vec; 430 | } 431 | 432 | public static Vector3i operator &(Vector3i vec, int and) 433 | { 434 | vec.x &= and; 435 | vec.y &= and; 436 | vec.z &= and; 437 | return vec; 438 | } 439 | 440 | public static Vector3i operator &(Vector3i vec, Vector3i andv) 441 | { 442 | vec.x &= andv.x; 443 | vec.y &= andv.y; 444 | vec.z &= andv.z; 445 | return vec; 446 | } 447 | 448 | public static bool operator ==(Vector3i left, Vector3i right) 449 | { 450 | return left.Equals(right); 451 | } 452 | 453 | public static bool operator !=(Vector3i left, Vector3i right) 454 | { 455 | return !left.Equals(right); 456 | } 457 | 458 | public static bool operator <(Vector3i left, Vector3i right) 459 | { 460 | if (left.x == right.x) 461 | { 462 | if (left.y == right.y) 463 | return left.z < right.z; 464 | else 465 | return left.y < right.y; 466 | } 467 | 468 | return left.x < right.x; 469 | } 470 | 471 | public static bool operator >(Vector3i left, Vector3i right) 472 | { 473 | if (left.x == right.x) 474 | { 475 | if (left.y == right.y) 476 | return left.z > right.z; 477 | else 478 | return left.y > right.y; 479 | } 480 | 481 | return left.x > right.x; 482 | } 483 | 484 | public static bool operator <=(Vector3i left, Vector3i right) 485 | { 486 | if (left.x == right.x) 487 | { 488 | if (left.y == right.y) 489 | return left.z <= right.z; 490 | else 491 | return left.y < right.y; 492 | } 493 | 494 | return left.x < right.x; 495 | } 496 | 497 | public static bool operator >=(Vector3i left, Vector3i right) 498 | { 499 | if (left.x == right.x) 500 | { 501 | if (left.y == right.y) 502 | return left.z >= right.z; 503 | else 504 | return left.y > right.y; 505 | } 506 | 507 | return left.x > right.x; 508 | } 509 | 510 | public static implicit operator Vector3(Vector3i value) 511 | { 512 | return new Vector3(value.x, value.y, value.z); 513 | } 514 | 515 | public static explicit operator Vector3i(Vector3 value) 516 | { 517 | return new Vector3i(value); 518 | } 519 | 520 | #if UNITY_5_3_OR_NEWER 521 | public static implicit operator Vector3Int(Vector3i value) 522 | { 523 | return new Vector3Int(value.x, value.y, value.z); 524 | } 525 | 526 | public static explicit operator Vector3i(Vector3Int value) 527 | { 528 | return new Vector3i(value); 529 | } 530 | #endif 531 | 532 | public override bool Equals(object obj) 533 | { 534 | if (obj is Vector3i) 535 | { 536 | return Equals((Vector3i)obj); 537 | } 538 | 539 | return false; 540 | } 541 | 542 | public bool Equals(Vector3i other) 543 | { 544 | return x == other.x && y == other.y && z == other.z; 545 | } 546 | 547 | public override int GetHashCode() 548 | { 549 | return y.GetHashCode() ^ x.GetHashCode() ^ z.GetHashCode(); 550 | } 551 | 552 | public override string ToString() 553 | { 554 | return String.Format("({0}, {1}, {2})", new object[] 555 | { 556 | this.x.ToString(), 557 | this.y.ToString(), 558 | this.z.ToString() 559 | }); 560 | } 561 | 562 | public string ToString(string format) 563 | { 564 | return String.Format("({0}, {1}, {2})", new object[] 565 | { 566 | this.x.ToString(format), 567 | this.y.ToString(format), 568 | this.z.ToString(format) 569 | }); 570 | } 571 | } 572 | } 573 | -------------------------------------------------------------------------------- /addons/extra_math_cs/ExtraMath/Integer/Vector4i.cs: -------------------------------------------------------------------------------- 1 | #if GODOT 2 | using Godot; 3 | #elif UNITY_5_3_OR_NEWER 4 | using UnityEngine; 5 | #endif 6 | using System; 7 | using System.Runtime.InteropServices; 8 | 9 | #if GODOT_REAL_T_IS_DOUBLE 10 | using real_t = System.Double; 11 | #else 12 | using real_t = System.Single; 13 | #endif 14 | 15 | namespace ExtraMath 16 | { 17 | /// 18 | /// 3-element structure that can be used to represent 3D grid coordinates or sets of integers. 19 | /// 20 | [Serializable] 21 | [StructLayout(LayoutKind.Sequential)] 22 | public struct Vector4i : IEquatable 23 | { 24 | public enum Axis 25 | { 26 | X = 0, 27 | Y, 28 | Z, 29 | W 30 | } 31 | 32 | public int x; 33 | public int y; 34 | public int z; 35 | public int w; 36 | 37 | public Vector3i XYZ 38 | { 39 | get 40 | { 41 | return new Vector3i(x, y, z); 42 | } 43 | set 44 | { 45 | x = value.x; 46 | y = value.y; 47 | z = value.z; 48 | } 49 | } 50 | 51 | public int this[int index] 52 | { 53 | get 54 | { 55 | switch (index) 56 | { 57 | case 0: 58 | return x; 59 | case 1: 60 | return y; 61 | case 2: 62 | return z; 63 | case 3: 64 | return w; 65 | default: 66 | throw new IndexOutOfRangeException(); 67 | } 68 | } 69 | set 70 | { 71 | switch (index) 72 | { 73 | case 0: 74 | x = value; 75 | return; 76 | case 1: 77 | y = value; 78 | return; 79 | case 2: 80 | z = value; 81 | return; 82 | case 3: 83 | w = value; 84 | return; 85 | default: 86 | throw new IndexOutOfRangeException(); 87 | } 88 | } 89 | } 90 | 91 | public Vector4i Abs() 92 | { 93 | return new Vector4i(Mathf.Abs(x), Mathf.Abs(y), Mathf.Abs(z), Mathf.Abs(w)); 94 | } 95 | 96 | public int DistanceSquaredTo(Vector4i b) 97 | { 98 | return (b - this).LengthSquared(); 99 | } 100 | 101 | public real_t DistanceTo(Vector4i b) 102 | { 103 | return (b - this).Length(); 104 | } 105 | 106 | public int Dot(Vector4i b) 107 | { 108 | return x * b.x + y * b.y + z * b.z + w * b.w; 109 | } 110 | 111 | public real_t Length() 112 | { 113 | int x2 = x * x; 114 | int y2 = y * y; 115 | int z2 = z * z; 116 | int w2 = w * w; 117 | 118 | return Mathf.Sqrt(x2 + y2 + z2 + w2); 119 | } 120 | 121 | public int LengthSquared() 122 | { 123 | int x2 = x * x; 124 | int y2 = y * y; 125 | int z2 = z * z; 126 | int w2 = w * w; 127 | 128 | return x2 + y2 + z2 + w2; 129 | } 130 | 131 | public Axis MaxAxis() 132 | { 133 | byte index = 0; 134 | for (byte i = 1; i < 4; i++) 135 | { 136 | if (this[i] > this[index]) 137 | { 138 | index = i; 139 | } 140 | } 141 | return (Axis)index; 142 | } 143 | 144 | public Axis MinAxis() 145 | { 146 | byte index = 0; 147 | for (byte i = 1; i < 4; i++) 148 | { 149 | if (this[i] < this[index]) 150 | { 151 | index = i; 152 | } 153 | } 154 | return (Axis)index; 155 | } 156 | 157 | #if GODOT 158 | public Vector4i PosMod(int mod) 159 | { 160 | Vector4i v = this; 161 | v.x = Mathf.PosMod(v.x, mod); 162 | v.y = Mathf.PosMod(v.y, mod); 163 | v.z = Mathf.PosMod(v.z, mod); 164 | v.w = Mathf.PosMod(v.w, mod); 165 | return v; 166 | } 167 | 168 | public Vector4i PosMod(Vector4i modv) 169 | { 170 | Vector4i v = this; 171 | v.x = Mathf.PosMod(v.x, modv.x); 172 | v.y = Mathf.PosMod(v.y, modv.y); 173 | v.z = Mathf.PosMod(v.z, modv.z); 174 | v.w = Mathf.PosMod(v.w, modv.w); 175 | return v; 176 | } 177 | #endif 178 | 179 | public Vector4i Sign() 180 | { 181 | Vector4i v = this; 182 | #if GODOT 183 | v.x = Mathf.Sign(v.x); 184 | v.y = Mathf.Sign(v.y); 185 | v.z = Mathf.Sign(v.z); 186 | v.w = Mathf.Sign(v.w); 187 | #else 188 | v.x = v.x < 0 ? -1 : 1; 189 | v.y = v.y < 0 ? -1 : 1; 190 | v.z = v.z < 0 ? -1 : 1; 191 | v.w = v.w < 0 ? -1 : 1; 192 | #endif 193 | return v; 194 | } 195 | 196 | public Vector2i[] UnpackVector2() 197 | { 198 | Vector2i[] arr = new Vector2i[2]; 199 | arr[0] = new Vector2i(x, y); 200 | arr[1] = new Vector2i(z, w); 201 | return arr; 202 | } 203 | 204 | public void UnpackVector2(out Vector2i xy, out Vector2i zw) 205 | { 206 | xy = new Vector2i(x, y); 207 | zw = new Vector2i(z, w); 208 | } 209 | 210 | // Constants 211 | private static readonly Vector4i _zero = new Vector4i(0, 0, 0, 0); 212 | private static readonly Vector4i _one = new Vector4i(1, 1, 1, 1); 213 | private static readonly Vector4i _negOne = new Vector4i(-1, -1, -1, -1); 214 | 215 | private static readonly Vector4i _unitX = new Vector4i(1, 0, 0, 0); 216 | private static readonly Vector4i _unitY = new Vector4i(0, 1, 0, 0); 217 | private static readonly Vector4i _unitZ = new Vector4i(0, 0, 1, 0); 218 | private static readonly Vector4i _unitW = new Vector4i(0, 0, 0, 1); 219 | 220 | public static Vector4i Zero { get { return _zero; } } 221 | public static Vector4i One { get { return _one; } } 222 | public static Vector4i NegOne { get { return _negOne; } } 223 | 224 | public static Vector4i UnitX { get { return _unitX; } } 225 | public static Vector4i UnitY { get { return _unitY; } } 226 | public static Vector4i UnitZ { get { return _unitZ; } } 227 | public static Vector4i UnitW { get { return _unitW; } } 228 | 229 | // Constructors 230 | public Vector4i(int x, int y, int z, int w) 231 | { 232 | this.x = x; 233 | this.y = y; 234 | this.z = z; 235 | this.w = w; 236 | } 237 | public Vector4i(Vector4i v) 238 | { 239 | this.x = v.x; 240 | this.y = v.y; 241 | this.z = v.z; 242 | this.w = v.w; 243 | } 244 | public Vector4i(Vector4 v) 245 | { 246 | this.x = Mathf.RoundToInt(v.x); 247 | this.y = Mathf.RoundToInt(v.y); 248 | this.z = Mathf.RoundToInt(v.z); 249 | this.w = Mathf.RoundToInt(v.w); 250 | } 251 | public Vector4i(Vector3i xyz, int w) 252 | { 253 | x = xyz.x; 254 | y = xyz.y; 255 | z = xyz.z; 256 | this.w = w; 257 | } 258 | public Vector4i(Vector2i xy, Vector2i zw) 259 | { 260 | x = xy.x; 261 | y = xy.y; 262 | z = zw.x; 263 | w = zw.y; 264 | } 265 | 266 | public static implicit operator Vector4(Vector4i value) 267 | { 268 | return new Vector4(value.x, value.y, value.z, value.w); 269 | } 270 | 271 | public static explicit operator Vector4i(Vector4 value) 272 | { 273 | return new Vector4i(value); 274 | } 275 | 276 | #if UNITY_5_3_OR_NEWER 277 | public static implicit operator UnityEngine.Vector4(Vector4i value) 278 | { 279 | return new UnityEngine.Vector4(value.x, value.y, value.z, value.w); 280 | } 281 | 282 | public static explicit operator Vector4i(UnityEngine.Vector4 value) 283 | { 284 | return new Vector4i((Vector4)value); 285 | } 286 | #endif 287 | 288 | public static Vector4i operator +(Vector4i left, Vector4i right) 289 | { 290 | left.x += right.x; 291 | left.y += right.y; 292 | left.z += right.z; 293 | left.w += right.w; 294 | return left; 295 | } 296 | 297 | public static Vector4i operator -(Vector4i left, Vector4i right) 298 | { 299 | left.x -= right.x; 300 | left.y -= right.y; 301 | left.z -= right.z; 302 | left.w -= right.w; 303 | return left; 304 | } 305 | 306 | public static Vector4i operator -(Vector4i vec) 307 | { 308 | vec.x = -vec.x; 309 | vec.y = -vec.y; 310 | vec.z = -vec.z; 311 | vec.w = -vec.w; 312 | return vec; 313 | } 314 | 315 | public static Vector4i operator *(Vector4i vec, int scale) 316 | { 317 | vec.x *= scale; 318 | vec.y *= scale; 319 | vec.z *= scale; 320 | vec.w *= scale; 321 | return vec; 322 | } 323 | 324 | public static Vector4i operator *(int scale, Vector4i vec) 325 | { 326 | vec.x *= scale; 327 | vec.y *= scale; 328 | vec.z *= scale; 329 | vec.w *= scale; 330 | return vec; 331 | } 332 | 333 | public static Vector4i operator *(Vector4i left, Vector4i right) 334 | { 335 | left.x *= right.x; 336 | left.y *= right.y; 337 | left.z *= right.z; 338 | left.w *= right.w; 339 | return left; 340 | } 341 | 342 | public static Vector4i operator /(Vector4i vec, int scale) 343 | { 344 | vec.x /= scale; 345 | vec.y /= scale; 346 | vec.z /= scale; 347 | vec.w /= scale; 348 | return vec; 349 | } 350 | 351 | public static Vector4i operator /(Vector4i left, Vector4i right) 352 | { 353 | left.x /= right.x; 354 | left.y /= right.y; 355 | left.z /= right.z; 356 | left.w /= right.w; 357 | return left; 358 | } 359 | 360 | public static Vector4i operator %(Vector4i vec, int divisor) 361 | { 362 | vec.x %= divisor; 363 | vec.y %= divisor; 364 | vec.z %= divisor; 365 | vec.w %= divisor; 366 | return vec; 367 | } 368 | 369 | public static Vector4i operator %(Vector4i vec, Vector4i divisorv) 370 | { 371 | vec.x %= divisorv.x; 372 | vec.y %= divisorv.y; 373 | vec.z %= divisorv.z; 374 | vec.w %= divisorv.w; 375 | return vec; 376 | } 377 | 378 | public static Vector4i operator &(Vector4i vec, int and) 379 | { 380 | vec.x &= and; 381 | vec.y &= and; 382 | vec.z &= and; 383 | vec.w &= and; 384 | return vec; 385 | } 386 | 387 | public static Vector4i operator &(Vector4i vec, Vector4i andv) 388 | { 389 | vec.x &= andv.x; 390 | vec.y &= andv.y; 391 | vec.z &= andv.z; 392 | vec.w &= andv.w; 393 | return vec; 394 | } 395 | 396 | public static bool operator ==(Vector4i left, Vector4i right) 397 | { 398 | return left.Equals(right); 399 | } 400 | 401 | public static bool operator !=(Vector4i left, Vector4i right) 402 | { 403 | return !left.Equals(right); 404 | } 405 | 406 | public static bool operator <(Vector4i left, Vector4i right) 407 | { 408 | if (left.x == right.x) 409 | { 410 | if (left.y == right.y) 411 | { 412 | if (left.z == right.z) 413 | { 414 | return left.w < right.w; 415 | } 416 | return left.z < right.z; 417 | } 418 | return left.y < right.y; 419 | } 420 | return left.x < right.x; 421 | } 422 | 423 | public static bool operator >(Vector4i left, Vector4i right) 424 | { 425 | if (left.x == right.x) 426 | { 427 | if (left.y == right.y) 428 | { 429 | if (left.z == right.z) 430 | { 431 | return left.w > right.w; 432 | } 433 | return left.z > right.z; 434 | } 435 | return left.y > right.y; 436 | } 437 | return left.x > right.x; 438 | } 439 | 440 | public static bool operator <=(Vector4i left, Vector4i right) 441 | { 442 | if (left.x == right.x) 443 | { 444 | if (left.y == right.y) 445 | { 446 | if (left.z == right.z) 447 | { 448 | return left.w <= right.w; 449 | } 450 | return left.z < right.z; 451 | } 452 | return left.y < right.y; 453 | } 454 | return left.x < right.x; 455 | } 456 | 457 | public static bool operator >=(Vector4i left, Vector4i right) 458 | { 459 | if (left.x == right.x) 460 | { 461 | if (left.y == right.y) 462 | { 463 | if (left.z == right.z) 464 | { 465 | return left.w >= right.w; 466 | } 467 | return left.z > right.z; 468 | } 469 | return left.y > right.y; 470 | } 471 | return left.x > right.x; 472 | } 473 | 474 | public override bool Equals(object obj) 475 | { 476 | if (obj is Vector4i) 477 | { 478 | return Equals((Vector4i)obj); 479 | } 480 | 481 | return false; 482 | } 483 | 484 | public bool Equals(Vector4i other) 485 | { 486 | return x == other.x && y == other.y && z == other.z && w == other.w; 487 | } 488 | 489 | public override int GetHashCode() 490 | { 491 | return y.GetHashCode() ^ x.GetHashCode() ^ z.GetHashCode() ^ w.GetHashCode(); 492 | } 493 | 494 | public override string ToString() 495 | { 496 | return String.Format("({0}, {1}, {2}, {3})", new object[] 497 | { 498 | x.ToString(), 499 | y.ToString(), 500 | z.ToString(), 501 | w.ToString() 502 | }); 503 | } 504 | 505 | public string ToString(string format) 506 | { 507 | return String.Format("({0}, {1}, {2}, {3})", new object[] 508 | { 509 | x.ToString(format), 510 | y.ToString(format), 511 | z.ToString(format), 512 | w.ToString(format) 513 | }); 514 | } 515 | } 516 | } 517 | -------------------------------------------------------------------------------- /addons/extra_math_cs/ExtraMath/Transform25D.cs: -------------------------------------------------------------------------------- 1 | #if GODOT 2 | using Godot; 3 | #elif UNITY_5_3_OR_NEWER 4 | using UnityEngine; 5 | #endif 6 | using System; 7 | using System.Runtime.InteropServices; 8 | 9 | namespace ExtraMath 10 | { 11 | /// 12 | /// Calculates the 2D transformation from a 3D position and a Basis25D. 13 | /// 14 | [Serializable] 15 | [StructLayout(LayoutKind.Sequential)] 16 | public struct Transform25D : IEquatable 17 | { 18 | // Public fields store information that is used to calculate the properties. 19 | 20 | /// 21 | /// Controls how the 3D position is transformed into 2D. 22 | /// 23 | public Basis25D basis; 24 | 25 | /// 26 | /// The 3D position of the object. Should be updated on every frame before everything else. 27 | /// 28 | public Vector3 spatialPosition; 29 | 30 | // Public properties calculate on-the-fly. 31 | 32 | #if GODOT 33 | /// 34 | /// The 2D transformation of this object. Slower than FlatPosition. 35 | /// 36 | public Transform2D FlatTransform 37 | { 38 | get 39 | { 40 | return new Transform2D(0, FlatPosition); 41 | } 42 | } 43 | #endif 44 | 45 | /// 46 | /// The 2D position of this object. 47 | /// 48 | public Vector2 FlatPosition 49 | { 50 | get 51 | { 52 | Vector2 pos = spatialPosition.x * basis.x; 53 | pos += spatialPosition.y * basis.y; 54 | pos += spatialPosition.z * basis.z; 55 | return pos; 56 | } 57 | } 58 | 59 | // Constructors 60 | public Transform25D(Transform25D transform25D) 61 | { 62 | basis = transform25D.basis; 63 | spatialPosition = transform25D.spatialPosition; 64 | } 65 | 66 | public Transform25D(Basis25D basis25D) 67 | { 68 | basis = basis25D; 69 | spatialPosition = new Vector3(); // Godot has "Zero", Unity has "zero", but both have parameterless constructors. 70 | } 71 | 72 | public Transform25D(Basis25D basis25D, Vector3 position3D) 73 | { 74 | basis = basis25D; 75 | spatialPosition = position3D; 76 | } 77 | 78 | public Transform25D(Vector2 xAxis, Vector2 yAxis, Vector2 zAxis) 79 | { 80 | basis = new Basis25D(xAxis, yAxis, zAxis); 81 | spatialPosition = new Vector3(); // Godot has "Zero", Unity has "zero", but both have parameterless constructors. 82 | } 83 | 84 | public Transform25D(Vector2 xAxis, Vector2 yAxis, Vector2 zAxis, Vector3 position3D) 85 | { 86 | basis = new Basis25D(xAxis, yAxis, zAxis); 87 | spatialPosition = position3D; 88 | } 89 | 90 | public static bool operator ==(Transform25D left, Transform25D right) 91 | { 92 | return left.Equals(right); 93 | } 94 | 95 | public static bool operator !=(Transform25D left, Transform25D right) 96 | { 97 | return !left.Equals(right); 98 | } 99 | 100 | public override bool Equals(object obj) 101 | { 102 | if (obj is Transform25D) 103 | { 104 | return Equals((Transform25D)obj); 105 | } 106 | return false; 107 | } 108 | 109 | public bool Equals(Transform25D other) 110 | { 111 | return basis.Equals(other.basis) && spatialPosition.Equals(other.spatialPosition); 112 | } 113 | 114 | public override int GetHashCode() 115 | { 116 | return basis.GetHashCode() ^ spatialPosition.GetHashCode(); 117 | } 118 | 119 | public override string ToString() 120 | { 121 | string s = String.Format("({0}, {1})", new object[] 122 | { 123 | basis.ToString(), 124 | spatialPosition.ToString() 125 | }); 126 | return s; 127 | } 128 | 129 | public string ToString(string format) 130 | { 131 | string s = String.Format("({0}, {1})", new object[] 132 | { 133 | basis.ToString(format), 134 | spatialPosition.ToString(format) 135 | }); 136 | return s; 137 | } 138 | } 139 | } 140 | -------------------------------------------------------------------------------- /addons/extra_math_cs/LICENSE.md: -------------------------------------------------------------------------------- 1 | ## MIT License 2 | 3 | Copyright © 2019-2021 Aaron Franke, Godot Engine contributors (cf. GodotSharp authors as per instructions). 4 | 5 | * To see authors, run these commands in a clone of [the Godot repo](https://github.com/godotengine/godot) (a ZIP download does **not** work): 6 | 7 | * `git shortlog -n -s -- modules/mono/glue/GodotSharp/GodotSharp/Core` (or [view in browser](https://github.com/godotengine/godot/commits/master/modules/mono/glue/GodotSharp/GodotSharp/Core)) 8 | * `git shortlog -n -s -- modules/mono/glue/Managed/Files` (or [view in browser](https://github.com/godotengine/godot/commits/master/modules/mono/glue/Managed/Files)) 9 | * `git shortlog -n -s -- modules/mono/glue/cs_files` (or [view in browser](https://github.com/godotengine/godot/commits/master/modules/mono/glue/cs_files)) 10 | 11 | Permission is hereby granted, free of charge, to any person obtaining a copy 12 | of this software and associated documentation files (the "Software"), to deal 13 | in the Software without restriction, including without limitation the rights 14 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 15 | copies of the Software, and to permit persons to whom the Software is 16 | furnished to do so, subject to the following conditions: 17 | 18 | The above copyright notice and this permission notice shall be included in all 19 | copies or substantial portions of the Software. 20 | 21 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 22 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 23 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 24 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 25 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 26 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 27 | SOFTWARE. 28 | -------------------------------------------------------------------------------- /addons/extra_math_cs/extramath.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronfranke/GodotExtraMath/b08de3d1380f3568eeed6576e6e5ad00b3a6319b/addons/extra_math_cs/extramath.png -------------------------------------------------------------------------------- /addons/extra_math_cs/extramath.png.import: -------------------------------------------------------------------------------- 1 | [remap] 2 | 3 | importer="texture" 4 | type="StreamTexture" 5 | path="res://.import/extramath.png-c264ec7fdc723b1f1a5dac16b7f36451.stex" 6 | metadata={ 7 | "vram_texture": false 8 | } 9 | 10 | [deps] 11 | 12 | source_file="res://addons/extra_math_cs/extramath.png" 13 | dest_files=[ "res://.import/extramath.png-c264ec7fdc723b1f1a5dac16b7f36451.stex" ] 14 | 15 | [params] 16 | 17 | compress/mode=0 18 | compress/lossy_quality=0.7 19 | compress/hdr_mode=0 20 | compress/bptc_ldr=0 21 | compress/normal_map=0 22 | flags/repeat=0 23 | flags/filter=true 24 | flags/mipmaps=false 25 | flags/anisotropic=false 26 | flags/srgb=2 27 | process/fix_alpha_border=true 28 | process/premult_alpha=false 29 | process/HDR_as_SRGB=false 30 | process/invert_color=false 31 | stream=false 32 | size_limit=0 33 | detect_3d=true 34 | svg/scale=1.0 35 | -------------------------------------------------------------------------------- /addons/extra_math_cs/plugin.cfg: -------------------------------------------------------------------------------- 1 | [plugin] 2 | 3 | name="Godot Extra Math (C#)" 4 | description="Library for extra math types in C#" 5 | author="Aaron Franke" 6 | version="3.3" 7 | script="" 8 | --------------------------------------------------------------------------------