egra: checkbox cosmetix
[iv.d.git] / vmath.d
bloba5615d4ff0674e94cbdbf239700aee74ff026d22
1 /*
2 * coded by Ketmar // Invisible Vector <ketmar@ketmar.no-ip.org>
3 * Understanding is not required. Only obedience.
5 * This program is free software: you can redistribute it and/or modify
6 * it under the terms of the GNU General Public License as published by
7 * the Free Software Foundation, version 3 of the License ONLY.
9 * This program is distributed in the hope that it will be useful,
10 * but WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 * GNU General Public License for more details.
14 * You should have received a copy of the GNU General Public License
15 * along with this program. If not, see <http://www.gnu.org/licenses/>.
17 // various vector and matrix operations
18 // matrix should be compatible with OpenGL, but mostly untested
19 module iv.vmath /*is aliced*/;
20 import iv.alice;
22 //version = aabbtree_many_asserts;
23 version = aabbtree_query_count;
24 version = vmath_slow_normalize;
27 // ////////////////////////////////////////////////////////////////////////// //
28 version(vmath_double) {
29 alias VFloat = double;
30 } else {
31 alias VFloat = float;
33 enum VFloatNum(long v) = cast(VFloat)v;
34 enum VFloatNum(float v) = cast(VFloat)v;
35 enum VFloatNum(double v) = cast(VFloat)v;
36 enum VFloatNum(real v) = cast(VFloat)v;
39 // ////////////////////////////////////////////////////////////////////////// //
40 /// is `v` finite (i.e. not a nan and not an infinity)?
41 public bool isFiniteDbl (in double v) pure nothrow @trusted @nogc {
42 pragma(inline, true);
43 return (((*cast(const(ulong)*)&v)&0x7ff0_0000_0000_0000UL) != 0x7ff0_0000_0000_0000UL);
47 /// is `v` finite (i.e. not a nan and not an infinity)?
48 public bool isFiniteFlt (in float v) pure nothrow @trusted @nogc {
49 pragma(inline, true);
50 return (((*cast(const(uint)*)&v)&0x7f80_0000U) != 0x7f80_0000U);
54 // ////////////////////////////////////////////////////////////////////////// //
55 private template isGoodSwizzling(string s, string comps, int minlen, int maxlen) {
56 private static template hasChar(string str, char ch, uint idx=0) {
57 static if (idx >= str.length) enum hasChar = false;
58 else static if (str[idx] == ch) enum hasChar = true;
59 else enum hasChar = hasChar!(str, ch, idx+1);
61 private static template swz(string str, string comps, uint idx=0) {
62 static if (idx >= str.length) enum swz = true;
63 else static if (hasChar!(comps, str[idx])) enum swz = swz!(str, comps, idx+1);
64 else enum swz = false;
66 static if (s.length >= minlen && s.length <= maxlen) enum isGoodSwizzling = swz!(s, comps);
67 else enum isGoodSwizzling = false;
70 private template SwizzleCtor(string stn, string s) {
71 private static template buildCtor (string s, uint idx) {
72 static if (idx >= s.length) enum buildCtor = "";
73 else enum buildCtor = s[idx]~","~buildCtor!(s, idx+1);
75 enum SwizzleCtor = stn~"("~buildCtor!(s, 0)~")";
79 // ////////////////////////////////////////////////////////////////////////// //
80 // Values obtained from Wolfram Alpha. 116 bits ought to be enough for anybody.
81 // Wolfram Alpha LLC. 2011. Wolfram|Alpha. http://www.wolframalpha.com/input/?i=e+in+base+16 (access July 6, 2011).
82 enum real E_R = 0x1.5bf0a8b1457695355fb8ac404e7a8p+1L; /* e = 2.718281... */
83 enum real LOG2T_R = 0x1.a934f0979a3715fc9257edfe9b5fbp+1L; /* log2 10 = 3.321928... */
84 enum real LOG2E_R = 0x1.71547652b82fe1777d0ffda0d23a8p+0L; /* log2 e = 1.442695... */
85 enum real LOG2_R = 0x1.34413509f79fef311f12b35816f92p-2L; /* log10 2 = 0.301029... */
86 enum real LOG10E_R = 0x1.bcb7b1526e50e32a6ab7555f5a67cp-2L; /* log10 e = 0.434294... */
87 enum real LN2_R = 0x1.62e42fefa39ef35793c7673007e5fp-1L; /* ln 2 = 0.693147... */
88 enum real LN10_R = 0x1.26bb1bbb5551582dd4adac5705a61p+1L; /* ln 10 = 2.302585... */
89 enum real PI_R = 0x1.921fb54442d18469898cc51701b84p+1L; /* PI = 3.141592... */
90 enum real PI_2_R = PI_R/2; /* PI / 2 = 1.570796... */
91 enum real PI_4_R = PI_R/4; /* PI / 4 = 0.785398... */
92 enum real M_1_PI_R = 0x1.45f306dc9c882a53f84eafa3ea69cp-2L; /* 1 / PI = 0.318309... */
93 enum real M_2_PI_R = 2*M_1_PI_R; /* 2 / PI = 0.636619... */
94 enum real M_2_SQRTPI_R = 0x1.20dd750429b6d11ae3a914fed7fd8p+0L; /* 2 / sqrt(PI) = 1.128379... */
95 enum real SQRT2_R = 0x1.6a09e667f3bcc908b2fb1366ea958p+0L; /* sqrt(2) = 1.414213... */
96 enum real SQRT1_2_R = SQRT2_R/2; /* sqrt(1/2) = 0.707106... */
98 enum double E_D = cast(double)0x1.5bf0a8b1457695355fb8ac404e7a8p+1L; /* e = 2.718281... */
99 enum double LOG2T_D = cast(double)0x1.a934f0979a3715fc9257edfe9b5fbp+1L; /* log2 10 = 3.321928... */
100 enum double LOG2E_D = cast(double)0x1.71547652b82fe1777d0ffda0d23a8p+0L; /* log2 e = 1.442695... */
101 enum double LOG2_D = cast(double)0x1.34413509f79fef311f12b35816f92p-2L; /* log10 2 = 0.301029... */
102 enum double LOG10E_D = cast(double)0x1.bcb7b1526e50e32a6ab7555f5a67cp-2L; /* log10 e = 0.434294... */
103 enum double LN2_D = cast(double)0x1.62e42fefa39ef35793c7673007e5fp-1L; /* ln 2 = 0.693147... */
104 enum double LN10_D = cast(double)0x1.26bb1bbb5551582dd4adac5705a61p+1L; /* ln 10 = 2.302585... */
105 enum double PI_D = cast(double)0x1.921fb54442d18469898cc51701b84p+1L; /* PI = 3.141592... */
106 enum double PI_2_D = cast(double)(PI_R/2); /* PI / 2 = 1.570796... */
107 enum double PI_4_D = cast(double)(PI_R/4); /* PI / 4 = 0.785398... */
108 enum double M_1_PI_D = cast(double)0x1.45f306dc9c882a53f84eafa3ea69cp-2L; /* 1 / PI = 0.318309... */
109 enum double M_2_PI_D = cast(double)(2*M_1_PI_R); /* 2 / PI = 0.636619... */
110 enum double M_2_SQRTPI_D = cast(double)0x1.20dd750429b6d11ae3a914fed7fd8p+0L; /* 2 / sqrt(PI) = 1.128379... */
111 enum double SQRT2_D = cast(double)0x1.6a09e667f3bcc908b2fb1366ea958p+0L; /* sqrt(2) = 1.414213... */
112 enum double SQRT1_2_D = cast(double)(SQRT2_R/2); /* sqrt(1/2) = 0.707106... */
114 enum float E_F = cast(float)0x1.5bf0a8b1457695355fb8ac404e7a8p+1L; /* e = 2.718281... */
115 enum float LOG2T_F = cast(float)0x1.a934f0979a3715fc9257edfe9b5fbp+1L; /* log2 10 = 3.321928... */
116 enum float LOG2E_F = cast(float)0x1.71547652b82fe1777d0ffda0d23a8p+0L; /* log2 e = 1.442695... */
117 enum float LOG2_F = cast(float)0x1.34413509f79fef311f12b35816f92p-2L; /* log10 2 = 0.301029... */
118 enum float LOG10E_F = cast(float)0x1.bcb7b1526e50e32a6ab7555f5a67cp-2L; /* log10 e = 0.434294... */
119 enum float LN2_F = cast(float)0x1.62e42fefa39ef35793c7673007e5fp-1L; /* ln 2 = 0.693147... */
120 enum float LN10_F = cast(float)0x1.26bb1bbb5551582dd4adac5705a61p+1L; /* ln 10 = 2.302585... */
121 enum float PI_F = cast(float)0x1.921fb54442d18469898cc51701b84p+1L; /* PI = 3.141592... */
122 enum float PI_2_F = cast(float)(PI_R/2); /* PI / 2 = 1.570796... */
123 enum float PI_4_F = cast(float)(PI_R/4); /* PI / 4 = 0.785398... */
124 enum float M_1_PI_F = cast(float)0x1.45f306dc9c882a53f84eafa3ea69cp-2L; /* 1 / PI = 0.318309... */
125 enum float M_2_PI_F = cast(float)(2*M_1_PI_R); /* 2 / PI = 0.636619... */
126 enum float M_2_SQRTPI_F = cast(float)0x1.20dd750429b6d11ae3a914fed7fd8p+0L; /* 2 / sqrt(PI) = 1.128379... */
127 enum float SQRT2_F = cast(float)0x1.6a09e667f3bcc908b2fb1366ea958p+0L; /* sqrt(2) = 1.414213... */
128 enum float SQRT1_2_F = cast(float)(SQRT2_R/2); /* sqrt(1/2) = 0.707106... */
131 // ////////////////////////////////////////////////////////////////////////// //
132 private template IsKnownVMathConstant(string name) {
133 static if (name == "E" || name == "LOG2T" || name == "LOG2E" || name == "LOG2" || name == "LOG10E" ||
134 name == "LN2" || name == "LN10" || name == "PI" || name == "PI_2" || name == "PI_4" ||
135 name == "M_1_PI" || name == "M_2_PI" || name == "M_2_SQRTPI" || name == "SQRT2" || name == "SQRT1_2")
137 enum IsKnownVMathConstant = true;
138 } else {
139 enum IsKnownVMathConstant = false;
143 template ImportCoreMath(FloatType, T...) {
144 static assert(
145 (is(FloatType == float) || is(FloatType == const float) || is(FloatType == immutable float)) ||
146 (is(FloatType == double) || is(FloatType == const double) || is(FloatType == immutable double)),
147 "import type should be `float` or `double`");
148 private template InternalImport(T...) {
149 static if (T.length == 0) enum InternalImport = "";
150 else static if (is(typeof(T[0]) == string)) {
151 static if (T[0] == "fabs" || T[0] == "cos" || T[0] == "sin" || T[0] == "sqrt") {
152 enum InternalImport = "import core.math : "~T[0]~";"~InternalImport!(T[1..$]);
153 } else static if (T[0] == "min" || T[0] == "nmin") {
154 enum InternalImport = "static T "~T[0]~"(T) (in T a, in T b) { pragma(inline, true); return (a < b ? a : b); }"~InternalImport!(T[1..$]);
155 } else static if (T[0] == "max" || T[0] == "nmax") {
156 enum InternalImport = "static T "~T[0]~"(T) (in T a, in T b) { pragma(inline, true); return (a > b ? a : b); }"~InternalImport!(T[1..$]);
157 } else static if (IsKnownVMathConstant!(T[0])) {
158 static if (is(FloatType == float) || is(FloatType == const float) || is(FloatType == immutable float)) {
159 enum InternalImport = "enum "~T[0]~"="~T[0]~"_F;"~InternalImport!(T[1..$]);
160 } else {
161 enum InternalImport = "enum "~T[0]~"="~T[0]~"_D;"~InternalImport!(T[1..$]);
163 } else static if (T[0] == "isnan") {
164 enum InternalImport = "import core.stdc.math : isnan;"~InternalImport!(T[1..$]);
165 } else static if (T[0] == "isfinite") {
166 static if (is(FloatType == float) || is(FloatType == const float) || is(FloatType == immutable float)) {
167 enum InternalImport = "import iv.nanpay : isfinite = isFinite;"~InternalImport!(T[1..$]);
168 } else {
169 enum InternalImport = "import iv.nanpay : isfinite = isFiniteD;"~InternalImport!(T[1..$]);
171 } else static if (is(FloatType == float) || is(FloatType == const float) || is(FloatType == immutable float)) {
172 enum InternalImport = "import core.stdc.math : "~T[0]~"="~T[0]~"f;"~InternalImport!(T[1..$]);
173 } else {
174 enum InternalImport = "import core.stdc.math : "~T[0]~";"~InternalImport!(T[1..$]);
177 else static assert(0, "string expected");
179 static if (T.length > 0) {
180 enum ImportCoreMath = InternalImport!T;
181 } else {
182 enum ImportCoreMath = "{}";
187 // ////////////////////////////////////////////////////////////////////////// //
188 enum FLTEPS = 1e-6f;
189 enum DBLEPS = 1.0e-18;
190 template EPSILON(T) if (is(T == float) || is(T == double)) {
191 static if (is(T == float)) enum EPSILON = FLTEPS;
192 else static if (is(T == double)) enum EPSILON = DBLEPS;
193 else static assert(0, "wtf?!");
195 template SMALLEPSILON(T) if (is(T == float) || is(T == double)) {
196 static if (is(T == float)) enum SMALLEPSILON = 1e-5f;
197 else static if (is(T == double)) enum SMALLEPSILON = 1.0e-9;
198 else static assert(0, "wtf?!");
201 auto deg2rad(T:double) (T v) pure nothrow @safe @nogc {
202 pragma(inline, true);
203 static if (__traits(isFloating, T)) {
204 static if (is(T == float)) alias PI = PI_F; else alias PI = PI_D;
205 return cast(T)(v*cast(T)PI/cast(T)180);
206 } else {
207 static if (is(VFloat == float)) alias PI = PI_F; else alias PI = PI_D;
208 return cast(VFloat)(cast(VFloat)v*cast(VFloat)PI/cast(VFloat)180);
212 auto rad2deg(T:double) (T v) pure nothrow @safe @nogc {
213 pragma(inline, true);
214 static if (__traits(isFloating, T)) {
215 static if (is(T == float)) alias PI = PI_F; else alias PI = PI_D;
216 return cast(T)(v*cast(T)180/cast(T)PI);
217 } else {
218 static if (is(VFloat == float)) alias PI = PI_F; else alias PI = PI_D;
219 return cast(VFloat)(cast(VFloat)v*cast(VFloat)180/cast(VFloat)PI);
224 // ////////////////////////////////////////////////////////////////////////// //
225 alias vec2 = VecN!2;
226 alias vec3 = VecN!3;
227 //alias AABB2 = AABBImpl!vec2;
230 // ////////////////////////////////////////////////////////////////////////// //
231 template IsVector(VT) {
232 static if (is(VT == VecN!(D, T), ubyte D, T)) {
233 enum IsVector = (D == 2 || D == 3);
234 } else {
235 enum IsVector = false;
239 template IsVectorDim(VT, ubyte dim) {
240 static if (is(VT == VecN!(D, T), ubyte D, T)) {
241 enum IsVectorDim = (D == dim);
242 } else {
243 enum IsVectorDim = false;
247 template IsVector(VT, FT) {
248 static if (is(VT == VecN!(D, T), ubyte D, T)) {
249 enum IsVector = ((D == 2 || D == 3) && is(T == FT));
250 } else {
251 enum IsVector = false;
255 template IsVectorDim(VT, FT, ubyte dim) {
256 static if (is(VT == VecN!(D, T), ubyte D, T)) {
257 enum IsVectorDim = (D == dim && is(T == FT));
258 } else {
259 enum IsVectorDim = false;
263 template VectorDims(VT) {
264 static if (is(VT == VecN!(D, F), ubyte D, F)) {
265 enum VectorDims = D;
266 } else {
267 enum VectorDims = 0;
271 template VectorFloat(VT) {
272 static if (is(VT == VecN!(D, F), ubyte D, F)) {
273 alias VectorFloat = F;
274 } else {
275 static assert(0, "not a vector");
280 // ////////////////////////////////////////////////////////////////////////// //
281 align(1) struct VecN(ubyte dims, FloatType=VFloat) if (dims >= 2 && dims <= 3 && (is(FloatType == float) || is(FloatType == double))) {
282 align(1):
283 public:
284 static T nmin(T) (in T a, in T b) { pragma(inline, true); return (a < b ? a : b); }
285 static T nmax(T) (in T a, in T b) { pragma(inline, true); return (a > b ? a : b); }
287 public:
288 enum isVector(VT) = (is(VT == VecN!(2, FloatType)) || is(VT == VecN!(3, FloatType)));
289 enum isVector2(VT) = is(VT == VecN!(2, FloatType));
290 enum isVector3(VT) = is(VT == VecN!(3, FloatType));
291 enum isSameVector(VT) = is(VT == VecN!(dims, FloatType));
293 alias v2 = VecN!(2, FloatType);
294 alias v3 = VecN!(3, FloatType);
296 alias Me = typeof(this);
297 alias Float = FloatType;
298 alias Dims = dims;
299 enum Epsilon = EPSILON!Float;
301 public:
302 Float x = 0;
303 Float y = 0;
304 static if (dims >= 3) Float z = 0;
306 nothrow @safe:
307 string toString () const {
308 import std.string : format;
309 try {
310 static if (dims == 2) return "(%s,%s)".format(x, y);
311 else static if (dims == 3) return "(%s,%s,%s)".format(x, y, z);
312 else static assert(0, "invalid dimension count for vector");
313 } catch (Exception) {
314 assert(0);
318 //HACK!
319 inout(Float)* unsafePtr () inout nothrow @trusted @nogc { pragma(inline, true); return cast(typeof(return))&x; }
321 @nogc:
322 this (in Float[] c) @trusted {
323 x = (c.length >= 1 ? c.ptr[0] : 0);
324 y = (c.length >= 2 ? c.ptr[1] : 0);
325 static if (dims == 3) z = (c.length >= 3 ? c.ptr[2] : 0);
328 this (in Float ax, in Float ay) pure {
329 pragma(inline, true);
330 x = ax;
331 y = ay;
332 static if (dims == 3) z = 0;
335 this (in Float ax, in Float ay, in Float az) pure {
336 //pragma(inline, true);
337 x = ax;
338 y = ay;
339 static if (dims == 3) z = az;
342 this(VT) (in auto ref VT v) pure if (isVector!VT) {
343 //pragma(inline, true);
344 x = v.x;
345 y = v.y;
346 static if (dims == 3) {
347 static if (isVector3!VT) z = v.z; else z = 0;
351 static Me Zero () pure nothrow @safe @nogc { pragma(inline, true); return Me(0, 0); }
352 static Me Invalid () pure nothrow @safe @nogc { pragma(inline, true); return Me(Float.nan, Float.nan); }
354 // infinites are invalid too
355 @property bool valid () const nothrow @safe @nogc {
357 pragma(inline, true);
358 import core.stdc.math : isnan;
359 static if (dims == 2) return !isnan(x) && !isnan(y);
360 else static if (dims == 3) return !isnan(x) && !isnan(y) && !isnan(z);
361 else static assert(0, "invalid dimension count for vector");
363 // this also reject nans
364 pragma(inline, true);
365 import core.stdc.math : isfinite;
366 static if (dims == 2) return isfinite(x) && isfinite(y);
367 else static if (dims == 3) return isfinite(x) && isfinite(y) && isfinite(z);
368 else static assert(0, "invalid dimension count for vector");
371 // this also reject nans
372 @property bool isFinite () const nothrow @safe @nogc {
373 pragma(inline, true);
374 import core.stdc.math : isfinite;
375 static if (dims == 2) return isfinite(x) && isfinite(y);
376 else static if (dims == 3) return isfinite(x) && isfinite(y) && isfinite(z);
377 else static assert(0, "invalid dimension count for vector");
380 @property bool isZero () const nothrow @safe @nogc {
381 pragma(inline, true);
382 static if (dims == 2) return (x == 0 && y == 0);
383 else static if (dims == 3) return (x == 0 && y == 0 && z == 0);
384 else static assert(0, "invalid dimension count for vector");
387 @property bool isNearZero () const nothrow @safe @nogc {
388 pragma(inline, true);
389 mixin(ImportCoreMath!(Float, "fabs"));
390 static if (dims == 2) return (fabs(x) < EPSILON!Float && fabs(y) < EPSILON!Float);
391 else static if (dims == 3) return (fabs(x) < EPSILON!Float && fabs(y) < EPSILON!Float && fabs(z) < EPSILON!Float);
392 else static assert(0, "invalid dimension count for vector");
395 void set (in Float[] c...) @trusted {
396 x = (c.length >= 1 ? c.ptr[0] : 0);
397 y = (c.length >= 2 ? c.ptr[1] : 0);
398 static if (dims == 3) z = (c.length >= 3 ? c.ptr[2] : 0);
401 static if (dims == 2)
402 void set (in Float ax, in Float ay) {
403 //pragma(inline, true);
404 x = ax;
405 y = ay;
408 static if (dims == 3)
409 void set (in Float ax, in Float ay, in Float az) {
410 //pragma(inline, true);
411 x = ax;
412 y = ay;
413 z = az;
416 void opAssign(VT) (in auto ref VT v) if (isVector!VT) {
417 //pragma(inline, true);
418 x = v.x;
419 y = v.y;
420 static if (dims == 3) {
421 static if (isVector3!VT) z = v.z; else z = 0;
425 Float opIndex (usize idx) const {
426 pragma(inline, true);
427 static if (dims == 2) return (idx == 0 ? x : idx == 1 ? y : Float.nan);
428 else static if (dims == 3) return (idx == 0 ? x : idx == 1 ? y : idx == 2 ? z : Float.nan);
429 else static assert(0, "invalid dimension count for vector");
432 void opIndexAssign (Float v, usize idx) {
433 pragma(inline, true);
434 static if (dims == 2) { if (idx == 0) x = v; else if (idx == 1) y = v; }
435 else static if (dims == 3) { if (idx == 0) x = v; else if (idx == 1) y = v; else if (idx == 2) z = v; }
436 else static assert(0, "invalid dimension count for vector");
439 ref auto normalize () {
440 //pragma(inline, true);
441 mixin(ImportCoreMath!(double, "sqrt"));
442 version(vmath_slow_normalize) {
443 static if (dims == 2) immutable double len = sqrt(x*x+y*y);
444 else static if (dims == 3) immutable double len = sqrt(x*x+y*y+z*z);
445 else static assert(0, "invalid dimension count for vector");
446 x /= len;
447 y /= len;
448 static if (dims == 3) z /= len;
449 } else {
450 static if (dims == 2) immutable double invlen = 1.0/sqrt(x*x+y*y);
451 else static if (dims == 3) immutable double invlen = 1.0/sqrt(x*x+y*y+z*z);
452 else static assert(0, "invalid dimension count for vector");
453 x *= invlen;
454 y *= invlen;
455 static if (dims == 3) z *= invlen;
457 return this;
460 Float normalizeRetLength () {
461 //pragma(inline, true);
462 mixin(ImportCoreMath!(double, "sqrt"));
463 static if (dims == 2) immutable double len = sqrt(x*x+y*y);
464 else static if (dims == 3) immutable double len = sqrt(x*x+y*y+z*z);
465 else static assert(0, "invalid dimension count for vector");
466 version(vmath_slow_normalize) {
467 x /= len;
468 y /= len;
469 static if (dims == 3) z /= len;
470 } else {
471 immutable double invlen = 1.0/len;
472 x *= invlen;
473 y *= invlen;
474 static if (dims == 3) z *= invlen;
476 return cast(Float)len;
480 ref auto safeNormalize () pure {
481 //pragma(inline, true);
482 import std.math : sqrt;
483 static if (dims == 2) Float invlength = sqrt(x*x+y*y);
484 else static if (dims == 3) Float invlength = sqrt(x*x+y*y+z*z);
485 else static assert(0, "invalid dimension count for vector");
486 if (invlength >= EPSILON!Float) {
487 invlength = cast(Float)1/invlength;
488 x *= invlength;
489 y *= invlength;
490 static if (dims == 3) z *= invlength;
491 } else {
492 x = 1;
493 y = 0;
494 static if (dims == 3) z = 0;
496 return this;
500 ref auto opOpAssign(string op, VT) (in auto ref VT a) if (isVector!VT && (op == "+" || op == "-" || op == "*")) {
501 //pragma(inline, true);
502 mixin("x "~op~"= a.x;");
503 mixin("y "~op~"= a.y;");
504 static if (dims == 3 && isVector3!VT) mixin("z "~op~"= a.z;");
505 return this;
508 ref auto opOpAssign(string op) (Float a) if (op == "+" || op == "-" || op == "*") {
509 //pragma(inline, true);
510 mixin("x "~op~"= a;");
511 mixin("y "~op~"= a;");
512 static if (dims == 3) mixin("z "~op~"= a;");
513 return this;
516 ref auto opOpAssign(string op:"/") (Float a) {
517 //import std.math : abs;
518 //pragma(inline, true);
519 //a = (abs(a) >= EPSILON!Float ? 1.0/a : Float.nan);
520 version(all/*vmath_slow_normalize*/) {
521 x /= a;
522 y /= a;
523 static if (dims == 3) z /= a;
524 } else {
525 immutable double aa = 1.0/a;
526 x *= aa;
527 y *= aa;
528 static if (dims == 3) z *= aa;
530 return this;
533 static if (dims == 2) {
534 // radians
535 ref auto rotate (Float angle) {
536 pragma(inline, true);
537 mixin(ImportCoreMath!(Float, "cos", "sin"));
538 immutable Float c = cos(angle);
539 immutable Float s = sin(angle);
540 immutable Float nx = x*c-y*s;
541 immutable Float ny = x*s+y*c;
542 x = nx;
543 y = ny;
544 return this;
547 auto rotated (Float angle) const {
548 pragma(inline, true);
549 mixin(ImportCoreMath!(Float, "cos", "sin"));
550 immutable Float c = cos(angle);
551 immutable Float s = sin(angle);
552 return v2(x*c-y*s, x*s+y*c);
556 const:
557 // from `this` to `a`
558 auto lerp(VT) (in auto ref VT a, in Float t) if (isVector!VT) {
559 pragma(inline, true);
560 return this+(a-this)*t;
563 auto normalized () {
564 pragma(inline, true);
565 static if (dims == 2) return v2(x, y).normalize; else return v3(x, y, z).normalize;
569 auto safeNormalized () {
570 pragma(inline, true);
571 static if (dims == 2) return v2(x, y).safeNormalize; else return v3(x, y, z).safeNormalize;
575 @property Float length () {
576 pragma(inline, true);
577 mixin(ImportCoreMath!(Float, "sqrt"));
578 static if (dims == 2) return sqrt(x*x+y*y);
579 else static if (dims == 3) return sqrt(x*x+y*y+z*z);
580 else static assert(0, "invalid dimension count for vector");
583 @property double dbllength () {
584 pragma(inline, true);
585 mixin(ImportCoreMath!(double, "sqrt"));
586 static if (dims == 2) return sqrt(cast(double)x*cast(double)x+cast(double)y*cast(double)y);
587 else static if (dims == 3) return sqrt(cast(double)x*cast(double)x+cast(double)y*cast(double)y+cast(double)z*cast(double)z);
588 else static assert(0, "invalid dimension count for vector");
591 @property Float lengthSquared () {
592 pragma(inline, true);
593 static if (dims == 2) return x*x+y*y;
594 else static if (dims == 3) return x*x+y*y+z*z;
595 else static assert(0, "invalid dimension count for vector");
598 @property double dbllengthSquared () {
599 pragma(inline, true);
600 static if (dims == 2) return cast(double)x*cast(double)x+cast(double)y*cast(double)y;
601 else static if (dims == 3) return cast(double)x*cast(double)x+cast(double)y*cast(double)y+cast(double)z*cast(double)z;
602 else static assert(0, "invalid dimension count for vector");
605 // distance
606 Float distance(VT) (in auto ref VT a) if (isVector!VT) {
607 pragma(inline, true);
608 mixin(ImportCoreMath!(Float, "sqrt"));
609 static if (dims == 2) {
610 static if (isVector2!VT) return sqrt((x-a.x)*(x-a.x)+(y-a.y)*(y-a.y));
611 else static if (isVector3!VT) return sqrt((x-a.x)*(x-a.x)+(y-a.y)*(y-a.y)+a.z*a.z);
612 else static assert(0, "invalid dimension count for vector");
613 } else static if (dims == 3) {
614 static if (isVector2!VT) return sqrt((x-a.x)*(x-a.x)+(y-a.y)*(y-a.y)+z*z);
615 else static if (isVector3!VT) return sqrt((x-a.x)*(x-a.x)+(y-a.y)*(y-a.y)+(z-a.z)*(z-a.z));
616 else static assert(0, "invalid dimension count for vector");
617 } else {
618 static assert(0, "invalid dimension count for vector");
622 // distance
623 Float distanceSquared(VT) (in auto ref VT a) if (isVector!VT) {
624 pragma(inline, true);
625 static if (dims == 2) {
626 static if (isVector2!VT) return (x-a.x)*(x-a.x)+(y-a.y)*(y-a.y);
627 else static if (isVector3!VT) return (x-a.x)*(x-a.x)+(y-a.y)*(y-a.y)+a.z*a.z;
628 else static assert(0, "invalid dimension count for vector");
629 } else static if (dims == 3) {
630 static if (isVector2!VT) return (x-a.x)*(x-a.x)+(y-a.y)*(y-a.y)+z*z;
631 else static if (isVector3!VT) return (x-a.x)*(x-a.x)+(y-a.y)*(y-a.y)+(z-a.z)*(z-a.z);
632 else static assert(0, "invalid dimension count for vector");
633 } else {
634 static assert(0, "invalid dimension count for vector");
638 // distance
639 double dbldistance(VT) (in auto ref VT a) if (isVector!VT) {
640 pragma(inline, true);
641 mixin(ImportCoreMath!(double, "sqrt"));
642 return sqrt(dbldistanceSquared(a));
645 // distance
646 double dbldistanceSquared(VT) (in auto ref VT a) if (isVector!VT) {
647 pragma(inline, true);
648 static if (dims == 2) {
649 static if (isVector2!VT) return cast(double)(x-a.x)*cast(double)(x-a.x)+cast(double)(y-a.y)*cast(double)(y-a.y);
650 else static if (isVector3!VT) return cast(double)(x-a.x)*cast(double)(x-a.x)+cast(double)(y-a.y)*cast(double)(y-a.y)+cast(double)a.z*cast(double)a.z;
651 else static assert(0, "invalid dimension count for vector");
652 } else static if (dims == 3) {
653 static if (isVector2!VT) return cast(double)(x-a.x)*cast(double)(x-a.x)+cast(double)(y-a.y)*cast(double)(y-a.y)+cast(double)z*cast(double)z;
654 else static if (isVector3!VT) return cast(double)(x-a.x)*cast(double)(x-a.x)+cast(double)(y-a.y)*cast(double)(y-a.y)+cast(double)(z-a.z)*cast(double)(z-a.z);
655 else static assert(0, "invalid dimension count for vector");
656 } else {
657 static assert(0, "invalid dimension count for vector");
661 auto opBinary(string op, VT) (in auto ref VT a) if (isVector!VT && (op == "+" || op == "-")) {
662 pragma(inline, true);
663 static if (dims == 2 && isVector2!VT) mixin("return v2(x"~op~"a.x, y"~op~"a.y);");
664 else static if (dims == 2 && isVector3!VT) mixin("return v3(x"~op~"a.x, y"~op~"a.y, 0);");
665 else static if (dims == 3 && isVector2!VT) mixin("return v3(x"~op~"a.x, y"~op~"a.y, 0);");
666 else static if (dims == 3 && isVector3!VT) mixin("return v3(x"~op~"a.x, y"~op~"a.y, z"~op~"a.z);");
667 else static assert(0, "invalid dimension count for vector");
670 // vector elements operation
671 auto op(string opr, VT) (in auto ref VT a) if (isVector!VT && (opr == "+" || opr == "-" || opr == "*" || opr == "/")) {
672 pragma(inline, true);
673 static if (dims == 2) {
674 static if (isVector2!VT) mixin("return v2(x"~opr~"a.x, y"~opr~"a.y);");
675 else static if (isVector3!VT) mixin("return v2(x"~opr~"a.x, y"~opr~"a.y);");
676 else static assert(0, "invalid dimension count for vector");
677 } else static if (dims == 3) {
678 static if (isVector2!VT) mixin("return v3(x"~opr~"a.x, y"~opr~"a.y, 0);");
679 else static if (isVector3!VT) mixin("return v3(x"~opr~"a.x, y"~opr~"a.y, z"~opr~"a.z);");
680 else static assert(0, "invalid dimension count for vector");
681 } else {
682 static assert(0, "invalid dimension count for vector");
686 // dot product
687 Float opBinary(string op:"*", VT) (in auto ref VT a) if (isVector!VT) { pragma(inline, true); return dot(a); }
689 // cross product
690 auto opBinary(string op:"%", VT) (in auto ref VT a) if (isVector!VT) { pragma(inline, true); return cross(a); }
691 auto opBinary(string op:"^", VT) (in auto ref VT a) if (isVector!VT) { pragma(inline, true); return cross(a); }
693 static if (dims == 2) {
694 auto opBinary(string op:"%", VT) (VT.Float a) if (isVector!VT) { pragma(inline, true); return cross(a); }
695 auto opBinary(string op:"^", VT) (VT.Float a) if (isVector!VT) { pragma(inline, true); return cross(a); }
698 auto opBinary(string op) (Float a) if (op == "+" || op == "-" || op == "*") {
699 pragma(inline, true);
700 static if (dims == 2) mixin("return v2(x"~op~"a, y"~op~"a);");
701 else static if (dims == 3) mixin("return v3(x"~op~"a, y"~op~"a, z"~op~"a);");
702 else static assert(0, "invalid dimension count for vector");
705 auto opBinaryRight(string op:"*") (Float a) {
706 pragma(inline, true);
707 static if (dims == 2) mixin("return v2(x"~op~"a, y"~op~"a);");
708 else static if (dims == 3) mixin("return v3(x"~op~"a, y"~op~"a, z"~op~"a);");
709 else static assert(0, "invalid dimension count for vector");
712 auto opBinary(string op:"/") (Float a) {
713 pragma(inline, true);
714 //import std.math : abs;
715 //immutable Float a = (abs(aa) >= EPSILON!Float ? 1.0/aa : Float.nan);
716 //immutable Float a = cast(Float)1/aa; // 1/0 == inf
717 version(all/*vmath_slow_normalize*/) {
718 static if (dims == 2) return v2(x/a, y/a);
719 else static if (dims == 3) return v3(x/a, y/a, z/a);
720 else static assert(0, "invalid dimension count for vector");
721 } else {
722 a = cast(Float)1/a; // 1/0 == inf
723 static if (dims == 2) return v2(x*a, y*a);
724 else static if (dims == 3) return v3(x*a, y*a, z*a);
725 else static assert(0, "invalid dimension count for vector");
729 auto opBinaryRight(string op:"/") (Float aa) {
730 pragma(inline, true);
732 import std.math : abs;
733 static if (dims == 2) return v2((abs(x) >= EPSILON!Float ? aa/x : 0), (abs(y) >= EPSILON!Float ? aa/y : 0));
734 else static if (dims == 3) return v3((abs(x) >= EPSILON!Float ? aa/x : 0), (abs(y) >= EPSILON!Float ? aa/y : 0), (abs(z) >= EPSILON!Float ? aa/z : 0));
735 else static assert(0, "invalid dimension count for vector");
737 static if (dims == 2) return v2(aa/x, aa/y);
738 else static if (dims == 3) return v3(aa/x, aa/y, aa/z);
739 else static assert(0, "invalid dimension count for vector");
742 auto opUnary(string op:"+") () { pragma(inline, true); return this; }
744 auto opUnary(string op:"-") () {
745 pragma(inline, true);
746 static if (dims == 2) return v2(-x, -y);
747 else static if (dims == 3) return v3(-x, -y, -z);
748 else static assert(0, "invalid dimension count for vector");
751 // this method performs the following triple product: (this x b) x c
752 Me tripleProduct() (in auto ref Me b, in auto ref Me c) {
753 alias a = this;
754 static if (dims == 2) {
755 // perform a.dot(c)
756 immutable Float ac = a.x*c.x+a.y*c.y;
757 // perform b.dot(c)
758 immutable Float bc = b.x*c.x+b.y*c.y;
759 // perform b * a.dot(c) - a * b.dot(c)
760 return Me(
761 b.x*ac-a.x*bc,
762 b.y*ac-a.y*bc,
764 } else {
765 // perform a.dot(c)
766 immutable Float ac = a.x*c.x+a.y*c.y+a.z*c.z;
767 // perform b.dot(c)
768 immutable Float bc = b.x*c.x+b.y*c.y+b.z*c.z;
769 // perform b * a.dot(c) - a * b.dot(c)
770 return Me(
771 b.x*ac-a.x*bc,
772 b.y*ac-a.y*bc,
773 b.z*ac-a.z*bc,
778 auto abs () {
779 pragma(inline, true);
780 mixin(ImportCoreMath!(Float, "fabs"));
781 static if (dims == 2) return v2(fabs(x), fabs(y));
782 else static if (dims == 3) return v3(fabs(x), fabs(y), fabs(z));
783 else static assert(0, "invalid dimension count for vector");
786 auto sign () {
787 pragma(inline, true);
788 static if (dims == 2) return v2((x < 0 ? -1 : x > 0 ? 1 : 0), (y < 0 ? -1 : y > 0 ? 1 : 0));
789 else static if (dims == 3) return v3((x < 0 ? -1 : x > 0 ? 1 : 0), (y < 0 ? -1 : y > 0 ? 1 : 0), (z < 0 ? -1 : z > 0 ? 1 : 0));
790 else static assert(0, "invalid dimension count for vector");
793 // `this` is edge; see glsl reference
794 auto step(VT) (in auto ref VT val) if (IsVector!VT) {
795 pragma(inline, true);
796 static if (dims == 2) return v2((val.x < this.x ? 0f : 1f), (val.y < this.y ? 0f : 1f));
797 else static if (dims == 3) {
798 static if (VT.Dims == 3) {
799 return v3((val.x < this.x ? 0f : 1f), (val.y < this.y ? 0f : 1f), (val.z < this.z ? 0f : 1f));
800 } else {
801 return v3((val.x < this.x ? 0f : 1f), (val.y < this.y ? 0f : 1f), (0 < this.z ? 0f : 1f));
804 else static assert(0, "invalid dimension count for vector");
807 bool opEquals(VT) (in auto ref VT a) if (isVector!VT) {
808 pragma(inline, true);
809 static if (dims == 2 && isVector2!VT) return (x == a.x && y == a.y);
810 else static if (dims == 2 && isVector3!VT) return (x == a.x && y == a.y && a.z == 0);
811 else static if (dims == 3 && isVector2!VT) return (x == a.x && y == a.y && z == 0);
812 else static if (dims == 3 && isVector3!VT) return (x == a.x && y == a.y && z == a.z);
813 else static assert(0, "invalid dimension count for vector");
816 // this dot a
817 @property Float dot(VT) (in auto ref VT a) if (isVector!VT) {
818 pragma(inline, true);
819 static if (dims == 2) {
820 return x*a.x+y*a.y;
821 } else static if (dims == 3) {
822 static if (isVector2!VT) return x*a.x+y*a.y;
823 else static if (isVector3!VT) return x*a.x+y*a.y+z*a.z;
824 else static assert(0, "invalid dimension count for vector");
825 } else {
826 static assert(0, "invalid dimension count for vector");
830 // this cross a
831 auto cross(VT) (in auto ref VT a) if (isVector!VT) {
832 pragma(inline, true);
833 static if (dims == 2 && isVector2!VT) return /*v3(0, 0, x*a.y-y*a.x)*/x*a.y-y*a.x;
834 else static if (dims == 2 && isVector3!VT) return v3(y*a.z, -x*a.z, x*a.y-y*a.x);
835 else static if (dims == 3 && isVector2!VT) return v3(-z*a.y, z*a.x, x*a.y-y*a.x);
836 else static if (dims == 3 && isVector3!VT) return v3(y*a.z-z*a.y, z*a.x-x*a.z, x*a.y-y*a.x);
837 else static assert(0, "invalid dimension count for vector");
840 // this*s; if you want s*this, do cross(-s)
841 static if (dims == 2) auto cross (Float s) {
842 pragma(inline, true);
843 return Me(s*y, -s*x);
846 // compute Euler angles from direction vector (this) (with zero roll)
847 auto hpr () {
848 auto tmp = this.normalized;
849 /*hpr.x = -atan2(tmp.x, tmp.y);
850 hpr.y = -atan2(tmp.z, sqrt(tmp.x*tmp.x+tmp.y*tmp.y));*/
851 static if (dims == 2) {
852 mixin(ImportCoreMath!(double, "atan2"));
853 return v2(
854 cast(Float)(atan2(cast(double)tmp.x, cast(Float)0)),
855 cast(Float)(-atan2(cast(double)tmp.y, cast(double)tmp.x)),
857 } else {
858 mixin(ImportCoreMath!(double, "atan2", "sqrt"));
859 return v3(
860 cast(Float)(atan2(cast(double)tmp.x, cast(double)tmp.z)),
861 cast(Float)(-atan2(cast(double)tmp.y, cast(double)sqrt(tmp.x*tmp.x+tmp.z*tmp.z))),
867 // some more supplementary functions to support various things
868 Float vcos(VT) (in auto ref VT v) if (isVector!VT) {
869 immutable double len = length*v.length;
870 return cast(Float)(len > EPSILON!Float ? cast(Float)(dot(v)/len) : cast(Float)0);
873 static if (dims == 2) Float vsin(VT) (in auto ref VT v) if (isVector!VT) {
874 immutable double len = length*v.length;
875 return cast(Float)(len > EPSILON!Float ? cast(Float)(cross(v)/len) : cast(Float)0);
878 static if (dims == 2) Float angle180(VT) (in auto ref VT v) if (isVector!VT) {
879 import std.math : PI;
880 mixin(ImportCoreMath!(Float, "atan"));
881 immutable Float cosv = vcos(v);
882 immutable Float sinv = vsin(v);
883 if (cosv == 0) return (sinv <= 0 ? -90 : 90);
884 if (sinv == 0) return (cosv <= 0 ? 180 : 0);
885 Float angle = (cast(Float)180*atan(sinv/cosv))/cast(Float)PI;
886 if (cosv < 0) { if (angle > 0) angle -= 180; else angle += 180; }
887 return angle;
890 static if (dims == 2) Float angle360(VT) (in auto ref VT v) if (isVector!VT) {
891 import std.math : PI;
892 mixin(ImportCoreMath!(Float, "atan"));
893 immutable Float cosv = vcos(v);
894 immutable Float sinv = vsin(v);
895 if (cosv == 0) return (sinv <= 0 ? 270 : 90);
896 if (sinv == 0) return (cosv <= 0 ? 180 : 0);
897 Float angle = (cast(Float)180*atan(sinv/cosv))/cast(Float)PI;
898 if (cosv < 0) angle += 180;
899 if (angle < 0) angle += 360;
900 return angle;
903 Float relativeAngle(VT) (in auto ref VT v) if (isVector!VT) {
904 import std.math : PI;
905 mixin(ImportCoreMath!(Float, "acos"));
906 immutable Float cosv = vcos(v);
907 if (cosv <= -1) return PI;
908 if (cosv >= 1) return 0;
909 return acos(cosv);
912 bool touch(VT) (in auto ref VT v) if (isVector!VT) { pragma(inline, true); return (distance(v) < /*SMALL*/EPSILON!Float); }
913 bool touch(VT) (in auto ref VT v, in Float epsilon) if (isVector!VT) { pragma(inline, true); return (distance(v) < epsilon); }
915 // is `this` on left? (or on line)
916 bool onLeft(VT) (in auto ref VT v0, in auto ref VT v1) if (isVector!VT) {
917 pragma(inline, true);
918 return ((v1-v0).cross(this-v0) <= 0);
921 static if (dims == 2) {
922 // 2d stuff
923 // test if a point (`this`) is left/on/right of an infinite 2D line
924 // return:
925 // <0: on the right
926 // =0: on the line
927 // >0: on the left
928 Float side(VT) (in auto ref VT v0, in auto ref VT v1) const if (isVector2!VT) {
929 pragma(inline, true);
930 return ((v1.x-v0.x)*(this.y-v0.y)-(this.x-v0.x)*(v1.y-v0.y));
934 // is` this` inside?
935 bool inside(VT) (in auto ref VT v0, in auto ref VT v1, in auto ref VT v2) if (isVector!VT) {
936 if ((v1-v0).cross(this-v0) <= 0) return false;
937 if ((v2-v1).cross(this-v1) <= 0) return false;
938 return ((v0-v2).cross(this-v2) > 0);
941 // box2dlite port support
942 static if (dims == 2) {
943 // returns a perpendicular vector (90 degree rotation)
944 auto perp() () { pragma(inline, true); return v2(-y, x); }
946 // returns a perpendicular vector (-90 degree rotation)
947 auto rperp() () { pragma(inline, true); return v2(y, -x); }
949 // returns the vector projection of this onto v
950 auto projectTo() (in auto ref v2 v) { pragma(inline, true); return v*(this.dot(v)/v.dot(v)); }
952 // returns the unit length vector for the given angle (in radians)
953 auto forAngle (in Float a) { pragma(inline, true); mixin(ImportCoreMath!(Float, "cos", "sin")); return v2(cos(a), sin(a)); }
955 // returns the angular direction v is pointing in (in radians)
956 Float toAngle() () { pragma(inline, true); mixin(ImportCoreMath!(Float, "atan2")); return atan2(y, x); }
958 auto scross() (Float s) { pragma(inline, true); return v2(-s*y, s*x); }
960 // returns the closest point to `this` on the line segment from `a` to `b`, or invalid vector
961 // if `asseg` is false, "segment" is actually a line (infinite in both directions)
962 Me projectToSeg(bool asseg=true) (in auto ref Me a, in auto ref Me b) {
963 mixin(ImportCoreMath!(Float, "fabs"));
964 alias p = this;
965 immutable ab = b-a; // vector from a to b
966 // squared distance from a to b
967 immutable absq = ab.dot(ab);
968 if (fabs(absq) < Epsilon) return a; // a and b are the same point (roughly)
969 immutable ap = p-a; // vector from a to p
970 immutable t = ap.dot(ab)/absq;
971 static if (asseg) {
972 if (t < 0) return a; // "before" a on the line
973 if (t > 1) return b; // "after" b on the line
975 // projection lies "inbetween" a and b on the line
976 return a+t*ab;
979 // returns the closest point to `this` on the line segment from `a` to `b`, or invalid vector
980 // if `asseg` is false, "segment" is actually a line (infinite in both directions)
981 Me projectToSegT(bool asseg=true) (in auto ref Me a, in auto ref Me b, ref Float rest) {
982 mixin(ImportCoreMath!(Float, "fabs"));
983 alias p = this;
984 immutable ab = b-a; // vector from a to b
985 // squared distance from a to b
986 immutable absq = ab.dot(ab);
987 if (fabs(absq) < Epsilon) { rest = 0; return a; } // a and b are the same point (roughly)
988 immutable ap = p-a; // vector from a to p
989 immutable t = ap.dot(ab)/absq;
990 rest = t;
991 static if (asseg) {
992 if (t < 0) return a; // "before" a on the line
993 if (t > 1) return b; // "after" b on the line
995 // projection lies "inbetween" a and b on the line
996 return a+t*ab;
999 // returns `t` (normalized distance of projected point from `a`)
1000 Float projectToLineT() (in auto ref Me a, in auto ref Me b) {
1001 mixin(ImportCoreMath!(Float, "fabs"));
1002 alias p = this;
1003 immutable ab = b-a; // vector from a to b
1004 // squared distance from a to b
1005 immutable absq = ab.dot(ab);
1006 if (fabs(absq) < Epsilon) return cast(Float)0; // a and b are the same point (roughly)
1007 immutable ap = p-a; // vector from a to p
1008 return cast(Float)(ap.dot(ab)/absq);
1012 bool equals() (in auto ref Me v) {
1013 pragma(inline, true);
1014 mixin(ImportCoreMath!(Float, "fabs"));
1015 static if (dims == 2) return (fabs(x-v.x) < EPSILON!Float && fabs(y-v.y) < EPSILON!Float);
1016 else static if (dims == 3) return (fabs(x-v.x) < EPSILON!Float && fabs(y-v.y) < EPSILON!Float && fabs(z-v.z) < EPSILON!Float);
1017 else static assert(0, "invalid dimension count for vector");
1021 // swizzling
1022 auto opDispatch(string fld) ()
1023 if ((dims == 2 && isGoodSwizzling!(fld, "xy", 2, 3)) ||
1024 (dims == 3 && isGoodSwizzling!(fld, "xyz", 2, 3)))
1026 static if (fld.length == 2) {
1027 return mixin(SwizzleCtor!("v2", fld));
1028 } else {
1029 return mixin(SwizzleCtor!("v3", fld));
1033 static if (dims == 3) bool collinear() (in auto ref Me v1, in auto ref Me v2) {
1034 pragma(inline, true);
1035 mixin(ImportCoreMath!(Float, "fabs"));
1036 alias v0 = this;
1037 immutable Float cx = (v1.y-v0.y)*(v2.z-v0.z)-(v2.y-v0.y)*(v1.z-v0.z);
1038 immutable Float cy = (v2.x-v0.x)*(v1.z-v0.z)-(v1.x-v0.x)*(v2.z-v0.z);
1039 immutable Float cz = (v1.x-v0.x)*(v2.y-v0.y)-(v2.x-v0.x)*(v1.y-v0.y);
1040 return (fabs(cast(double)(cx*cx+cy*cy+cz*cz)) < EPSILON!double);
1043 static if (dims == 2) bool collinear() (in auto ref Me v1, in auto ref Me v2) {
1044 pragma(inline, true);
1045 mixin(ImportCoreMath!(double, "fabs"));
1046 alias v0 = this;
1047 immutable Float det = cast(Float)((v0-v1)*(v0-v2)-(v0-v2)*(v0-v1));
1048 return (fabs(det) <= EPSILON!Float);
1051 static if (dims == 2) {
1052 import std.range.primitives : isInputRange, ElementType;
1054 static auto vertRange (const(Me)[] varr) {
1055 static struct VertRange {
1056 const(Me)[] arr;
1057 usize pos;
1058 nothrow @trusted @nogc:
1059 @property bool empty () const pure { pragma(inline, true); return (pos >= arr.length); }
1060 @property Me front () const pure { pragma(inline, true); return (pos < arr.length ? arr.ptr[pos] : Me.Invalid); }
1061 void popFront () { pragma(inline, true); if (pos < arr.length) ++pos; }
1062 auto save () const pure { pragma(inline, true); return VertRange(arr, pos); }
1064 return VertRange(varr, 0);
1067 bool insidePoly(VR) (auto ref VR vr) const if (isInputRange!VR && is(ElementType!VR : const Me)) {
1068 if (vr.empty) return false;
1069 Me p1 = vr.front;
1070 vr.popFront();
1071 if (vr.empty) return false;
1072 Me p2 = vr.front;
1073 vr.popFront();
1074 if (vr.empty) return false; //TODO: check if p is ON the edge?
1075 alias p = this;
1076 int counter = 0;
1077 for (;;) {
1078 if (p.y > nmin(p1.y, p2.y)) {
1079 if (p.y <= nmax(p1.y, p2.y)) {
1080 if (p.x <= nmax(p1.x, p2.x)) {
1081 if (p1.y != p2.y) {
1082 auto xinters = (p.y-p1.y)*(p2.x-p1.x)/(p2.y-p1.y)+p1.x;
1083 if (p1.x == p2.x || p.x <= xinters) counter ^= 1;
1088 if (vr.empty) break;
1089 p1 = p2;
1090 p2 = vr.front;
1091 vr.popFront();
1093 return (counter != 0);
1096 bool insidePoly() (const(Me)[] poly) const { pragma(inline, true); return insidePoly(vertRange(poly)); }
1098 // gets the signed area
1099 // if the area is less than 0, it indicates that the polygon is clockwise winded
1100 Me.Float signedArea(VR) (auto ref VR vr) const if (isInputRange!VR && is(ElementType!VR : const Me)) {
1101 Me.Float area = 0;
1102 if (vr.empty) return area;
1103 Me p1 = vr.front;
1104 vr.popFront();
1105 if (vr.empty) return area;
1106 Me p2 = vr.front;
1107 vr.popFront();
1108 if (vr.empty) return area;
1109 Me pfirst = p1;
1110 for (;;) {
1111 area += p1.x*p2.y;
1112 area -= p1.y*p2.x;
1113 if (vr.empty) break;
1114 p1 = p2;
1115 p2 = vr.front;
1116 vr.popFront();
1118 // last and first
1119 area += p2.x*pfirst.y;
1120 area -= p2.y*pfirst.x;
1121 return area/cast(VT.Float)2;
1124 Me.Float signedArea() (const(Me)[] poly) const { pragma(inline, true); return signedArea(vertRange(poly)); }
1126 // indicates if the vertices are in counter clockwise order
1127 // warning: If the area of the polygon is 0, it is unable to determine the winding
1128 bool isCCW(VR) (auto ref VR vr) const if (isInputRange!VR && is(ElementType!VR : const Me)) { pragma(inline, true); return (signedArea(vr) > 0); }
1129 bool isCCW() (const(Me)[] poly) const { pragma(inline, true); return (signedArea(vertRange(poly)) > 0); }
1131 // *signed* area; can be used to check on which side `b` is
1132 static auto triSignedArea() (in auto ref Me a, in auto ref Me b, in auto ref Me c) {
1133 pragma(inline, true);
1134 return (b.x-a.x)*(c.y-a.y)-(c.x-a.x)*(b.y-a.y);
1136 } // dims2
1138 static if (dims == 3) {
1139 // project this point to the given line
1140 Me projectToLine() (in auto ref Me p0, in auto ref Me p1) const {
1141 immutable Me d = p1-p0;
1142 immutable Me.Float t = d.dot(this-p0)/d.dot(d);
1143 if (tout !is null) *tout = t;
1144 return p0+d*t;
1147 // return "time" of projection
1148 Me.Float projectToLineTime() (in auto ref Me p0, in auto ref Me p1) const {
1149 immutable Me d = p1-p0;
1150 return d.dot(this-p0)/d.dot(d);
1153 // calculate triangle normal
1154 static Me triangleNormal() (in auto ref Me v0, in auto ref Me v1, in auto ref Me v2) {
1155 mixin(ImportCoreMath!(Me.Float, "fabs"));
1156 immutable Me cp = (v1-v0).cross(v2-v1);
1157 immutable Me.Float m = cp.length;
1158 return (fabs(m) > Epsilon ? cp*(cast(Me.Float)1/m) : Me.init);
1161 // polygon must be convex, ccw, and without collinear vertices
1162 //FIXME: UNTESTED
1163 bool insideConvexPoly (const(Me)[] ply...) const @trusted {
1164 if (ply.length < 3) return false;
1165 immutable normal = triangleNormal(ply.ptr[0], ply.ptr[1], ply.ptr[2]);
1166 auto pidx = ply.length-1;
1167 for (typeof(pidx) cidx = 0; cidx < ply.length; pidx = cidx, ++cidx) {
1168 immutable pp1 = ply.ptr[pidx];
1169 immutable pp2 = ply.ptr[cidx];
1170 immutable side = (pp2-pp1).cross(this-pp1);
1171 if (normal.dot(side) < 0) return false;
1173 return true;
1177 static:
1178 // linearly interpolate between v1 and v2
1180 VT lerp(VT) (in auto ref VT v1, in auto ref VT v2, const Float t) if (isSameVector!VT) {
1181 pragma(inline, true);
1182 return (v1*cast(Float)1.0f-t)+(v2*t);
1186 static if (dims == 2) {
1187 Me lineIntersect() (in auto ref Me p1, in auto ref Me p2, in auto ref Me q1, in auto ref Me q2) {
1188 pragma(inline, true);
1189 mixin(ImportCoreMath!(Me.Float, "fabs"));
1190 immutable Me.Float a1 = p2.y-p1.y;
1191 immutable Me.Float b1 = p1.x-p2.x;
1192 immutable Me.Float c1 = a1*p1.x+b1*p1.y;
1193 immutable Me.Float a2 = q2.y-q1.y;
1194 immutable Me.Float b2 = q1.x-q2.x;
1195 immutable Me.Float c2 = a2*q1.x+b2*q1.y;
1196 immutable Me.Float det = a1*b2-a2*b1;
1197 if (fabs(det) > EPSILON!(Me.Float)) {
1198 // lines are not parallel
1199 immutable Me.Float invdet = cast(Me.Float)1/det;
1200 return Me((b2*c1-b1*c2)*invdet, (a1*c2-a2*c1)*invdet);
1202 return Me.Invalid;
1205 Me segIntersect(bool firstIsSeg=true, bool secondIsSeg=true) (in auto ref Me point0, in auto ref Me point1, in auto ref Me point2, in auto ref Me point3) {
1206 mixin(ImportCoreMath!(Me.Float, "fabs"));
1207 static if (firstIsSeg && secondIsSeg) {
1208 // fast aabb test for possible early exit
1209 if (nmax(point0.x, point1.x) < nmin(point2.x, point3.x) || nmax(point2.x, point3.x) < nmin(point0.x, point1.x)) return Me.Invalid;
1210 if (nmax(point0.y, point1.y) < nmin(point2.y, point3.y) || nmax(point2.y, point3.y) < nmin(point0.y, point1.y)) return Me.Invalid;
1212 immutable Me.Float den = ((point3.y-point2.y)*(point1.x-point0.x))-((point3.x-point2.x)*(point1.y-point0.y));
1213 if (fabs(den) > EPSILON!(Me.Float)) {
1214 immutable Me.Float e = point0.y-point2.y;
1215 immutable Me.Float f = point0.x-point2.x;
1216 immutable Me.Float invden = cast(Me.Float)1/den;
1217 immutable Me.Float ua = (((point3.x-point2.x)*e)-((point3.y-point2.y)*f))*invden;
1218 static if (firstIsSeg) { if (ua < 0 || ua > 1) return Me.Invalid; }
1219 if (ua >= 0 && ua <= 1) {
1220 immutable Me.Float ub = (((point1.x-point0.x)*e)-((point1.y-point0.y)*f))*invden;
1221 static if (secondIsSeg) { if (ub < 0 || ub > 1) return Me.Invalid; }
1222 if (ua != 0 || ub != 0) return Me(point0.x+ua*(point1.x-point0.x), point0.y+ua*(point1.y-point0.y));
1225 return Me.Invalid;
1228 // returns hittime; <0: no collision; 0: inside
1229 // WARNING! NOT REALLY TESTED, AND MAY BE INCORRECT!
1230 Me.Float sweepCircle() (in auto ref Me v0, in auto ref Me v1, in auto ref Me pos, Me.Float radii, in auto ref Me vel, ref Me hitp) {
1231 mixin(ImportCoreMath!(Me.Float, "fabs", "sqrt"));
1232 if (v0.equals(v1)) return (pos.distanceSquared(v0) <= radii*radii ? 0 : -1); // v0 and v1 are the same point; do "point inside circle" check
1233 immutable Me normal = (v1-v0).perp.normalized;
1234 immutable Me.Float D = -normal*((v0+v1)/2);
1235 immutable d0 = normal.dot(pos)+D;
1236 if (fabs(d0) <= radii) return cast(Me.Float)0; // inside
1237 // sweep to plane
1238 immutable p1 = pos+vel;
1239 immutable Me.Float d1 = normal.dot(p1)+D;
1240 if (d0 > radii && d1 < radii) {
1241 Me.Float t = (d0-radii)/(d0-d1); // normalized time
1242 hitp = pos+vel*t;
1243 // project hitp point to segment
1244 alias a = v0;
1245 alias b = v1;
1246 alias p = hitp;
1247 immutable ab = b-a; // vector from a to b
1248 // squared distance from a to b
1249 immutable absq = ab.dot(ab);
1250 //assert(absq != 0); // a and b are the same point
1251 if (fabs(absq) < Me.Epsilon) return -1; // a and b are the same point (roughly) -- SOMETHING IS VERY WRONG
1252 // t1 is projection "time" of p; [0..1]
1253 Me.Float t1 = (p-a).dot(ab)/absq; //a+t1*ab -- projected point
1254 // is "contact center" lies on the seg?
1255 if (t1 >= 0 && t1 <= 1) return t; // yes: this is clear hit
1256 // because i'm teh idiot, i'll just check ray-circle intersection for edge's capsue endpoints
1257 // ('cause if we'll turn edge into the capsule (v0,v1) with radius radii, we can use raycasting)
1258 // this is not entirely valid for segments much shorter than radius, but meh
1259 Me.Float ct1;
1260 immutable Me eco = (t1 < 0 ? a : b);
1261 immutable Me rpj = eco.projectToSegT!false(pos, p1, ct1);
1262 immutable Me.Float dsq = eco.distanceSquared(rpj);
1263 if (dsq >= radii*radii) return -1; // endpoint may be on sphere or out of it, this is not interesting
1264 immutable Me.Float dt = sqrt(radii*radii-dsq)/sqrt(vel.x*vel.x+vel.y*vel.y);
1265 t = ct1-fabs(dt);
1266 hitp = pos+t*vel;
1267 return t;
1269 // no collision
1270 return -1;
1276 // ////////////////////////////////////////////////////////////////////////// //
1277 // 2x2 matrix for fast 2D rotations and such
1278 alias mat22 = Mat22!vec2;
1280 align(1) struct Mat22(VT) if (IsVectorDim!(VT, 2)) {
1281 align(1):
1282 public:
1283 alias Float = VT.Float;
1284 alias mat22 = typeof(this);
1285 alias vec2 = VecN!(3, Float);
1286 alias Me = typeof(this);
1288 nothrow @safe @nogc:
1289 public:
1290 // default is "not rotated"
1291 vec2 col1 = vec2(1, 0);
1292 vec2 col2 = vec2(0, 1);
1294 public:
1295 this (Float angle) { pragma(inline, true); set(angle); }
1297 void set (Float angle) {
1298 pragma(inline, true);
1299 mixin(ImportCoreMath!(Float, "cos", "sin"));
1300 immutable Float c = cos(angle), s = sin(angle);
1301 col1.x = c; col1.y = s;
1302 col2.x = -s; col2.y = c;
1305 pure:
1306 this() (in auto ref vec2 acol1, in auto ref vec2 acol2) { pragma(inline, true); col1 = acol1; col2 = acol2; }
1308 static Me Identity () { pragma(inline, true); Me res; return res; }
1310 Me transpose () const { pragma(inline, true); return Me(vec2(col1.x, col2.x), vec2(col1.y, col2.y)); }
1312 Me invert () const @trusted {
1313 pragma(inline, true);
1314 immutable Float a = col1.x, b = col2.x, c = col1.y, d = col2.y;
1315 Me bm = void;
1316 Float det = a*d-b*c;
1317 assert(det != cast(Float)0);
1318 det = cast(Float)1/det;
1319 bm.col1.x = det*d;
1320 bm.col2.x = -det*b;
1321 bm.col1.y = -det*c;
1322 bm.col2.y = det*a;
1323 return bm;
1326 Me opUnary(string op:"+") () const { pragma(inline, true); return this; }
1327 Me opUnary(string op:"-") () const { pragma(inline, true); return Me(-col1, -col2); }
1329 vec2 opBinary(string op:"*") (in auto ref vec2 v) const { pragma(inline, true); return vec2(col1.x*v.x+col2.x*v.y, col1.y*v.x+col2.y*v.y); }
1330 vec2 opBinaryRight(string op:"*") (in auto ref vec2 v) const { pragma(inline, true); return vec2(col1.x*v.x+col2.x*v.y, col1.y*v.x+col2.y*v.y); }
1332 Me opBinary(string op:"*") (Float s) const { pragma(inline, true); return Me(vec2(col1*s, col2*s)); }
1333 Me opBinaryRight(string op:"*") (Float s) const { pragma(inline, true); return Me(vec2(col1*s, col2*s)); }
1335 Me opBinary(string op) (in auto ref Me bm) const if (op == "+" || op == "-") { pragma(inline, true); mixin("return Me(col1"~op~"bm.col1, col2"~op~"bm.col2);"); }
1336 Me opBinary(string op:"*") (in auto ref Me bm) const { pragma(inline, true); return Me(this*bm.col1, this*bm.col2); }
1338 ref Me opOpAssign(string op) (in auto ref Me bm) if (op == "+" || op == "-") { pragma(inline, true); mixin("col1 "~op~"= bm.col1;"); mixin("col2 "~op~"= bm.col2;"); return this; }
1340 ref Me opOpAssign(string op:"*") (Float s) const { pragma(inline, true); col1 *= s; col2 *= s; }
1342 Me abs() () { pragma(inline, true); return Me(col1.abs, col2.abs); }
1344 // solves the system of linear equations: Ax = b
1345 vec2 solve() (in auto ref vec2 v) const {
1346 immutable Float a = col1.x, b = col2.x, c = col1.y, d = col2.y;
1347 Float det = a*d-b*c;
1348 assert(det != cast(Float)0);
1349 det = cast(Float)1/det;
1350 return vec2((d*v.x-b*v.y)*det, (a*v.y-c*v.x)*det);
1355 // ////////////////////////////////////////////////////////////////////////// //
1356 alias mat3 = Mat3!vec2; /// for 2D
1357 alias mat33 = Mat3!vec3; /// for 3D
1359 /// very simple (and mostly not tested) 3x3 matrix, for 2D/3D (depenting of parameterizing vector type)
1360 /// for 3D, 3x3 matrix cannot do translation
1361 align(1) struct Mat3(VT) if (IsVectorDim!(VT, 2) || IsVectorDim!(VT, 3)) {
1362 align(1):
1363 private:
1364 alias Float = VT.Float;
1365 alias m3 = typeof(this);
1366 static if (VT.Dims == 2) {
1367 alias v2 = VT;
1368 enum TwoD = true;
1369 enum ThreeD = false;
1370 enum isVector2(VT) = is(VT == VecN!(2, Float));
1371 } else {
1372 alias v3 = VT;
1373 enum TwoD = false;
1374 enum ThreeD = true;
1375 enum isVector3(VT) = is(VT == VecN!(3, Float));
1378 private:
1379 // 3x3 matrix components
1380 Float[3*3] m = [
1381 1, 0, 0,
1382 0, 1, 0,
1383 0, 0, 1,
1386 public:
1387 string toString () const nothrow @trusted {
1388 import std.string : format;
1389 try {
1390 return "0:[%g,%g,%g]\n3:[%g,%g,%g]\n6:[%g,%g,%g]".format(
1391 m.ptr[0], m.ptr[1], m.ptr[2],
1392 m.ptr[3], m.ptr[4], m.ptr[5],
1393 m.ptr[6], m.ptr[7], m.ptr[8],
1395 } catch (Exception) {
1396 assert(0);
1400 public:
1401 nothrow @trusted @nogc:
1402 this (const(Float)[] vals...) {
1403 pragma(inline, true);
1404 if (vals.length >= 3*3) {
1405 m.ptr[0..9] = vals.ptr[0..9];
1406 } else {
1407 // so `m3(1)`, for example, will create matrix filled with `1`
1408 if (vals.length == 1) {
1409 m.ptr[0..9] = vals.ptr[0];
1410 } else {
1411 // still clear the matrix
1412 m.ptr[0..9] = 0;
1413 m.ptr[0..vals.length] = vals[];
1418 this() (in auto ref m3 mt) {
1419 pragma(inline, true);
1420 m[] = mt.m[];
1423 Float opIndex (usize x, usize y) const {
1424 pragma(inline, true);
1425 return (x < 3 && y < 3 ? m.ptr[y*3+x] : Float.nan);
1428 void opIndexAssign (Float v, usize x, usize y) {
1429 pragma(inline, true);
1430 if (x < 3 && y < 3) m.ptr[y*3+x] = v;
1433 @property bool isIdentity () const {
1434 pragma(inline, true); // can we?
1435 return
1436 m.ptr[0] == 1 && m.ptr[1] == 0 && m.ptr[2] == 0 &&
1437 m.ptr[3] == 0 && m.ptr[4] == 1 && m.ptr[5] == 0 &&
1438 m.ptr[6] == 0 && m.ptr[7] == 0 && m.ptr[8] == 1;
1441 auto opUnary(string op:"+") () const { pragma(inline, true); return this; }
1443 auto opUnary(string op:"-") () const {
1444 pragma(inline, true);
1445 return m3(
1446 -m.ptr[0], -m.ptr[1], -m.ptr[2],
1447 -m.ptr[3], -m.ptr[4], -m.ptr[5],
1448 -m.ptr[6], -m.ptr[7], -m.ptr[8],
1452 auto opBinary(string op) (in auto ref m3 b) const if (op == "+" || op == "-") {
1453 pragma(inline, true);
1454 m3 res = void;
1455 mixin("res.m.ptr[0] = m.ptr[0]"~op~"b.m.ptr[0];");
1456 mixin("res.m.ptr[1] = m.ptr[1]"~op~"b.m.ptr[1];");
1457 mixin("res.m.ptr[2] = m.ptr[2]"~op~"b.m.ptr[2];");
1458 mixin("res.m.ptr[3] = m.ptr[3]"~op~"b.m.ptr[3];");
1459 mixin("res.m.ptr[4] = m.ptr[4]"~op~"b.m.ptr[4];");
1460 mixin("res.m.ptr[5] = m.ptr[5]"~op~"b.m.ptr[5];");
1461 mixin("res.m.ptr[6] = m.ptr[6]"~op~"b.m.ptr[6];");
1462 mixin("res.m.ptr[7] = m.ptr[7]"~op~"b.m.ptr[7];");
1463 mixin("res.m.ptr[8] = m.ptr[8]"~op~"b.m.ptr[8];");
1464 return m3;
1467 ref auto opOpAssign(string op) (in auto ref m3 b) if (op == "+" || op == "-") {
1468 pragma(inline, true);
1469 mixin("m.ptr[0]"~op~"=b.m.ptr[0]; m.ptr[1]"~op~"=b.m.ptr[1]; m.ptr[2]"~op~"=b.m.ptr[2];");
1470 mixin("m.ptr[3]"~op~"=b.m.ptr[3]; m.ptr[4]"~op~"=b.m.ptr[4]; m.ptr[5]"~op~"=b.m.ptr[5];");
1471 mixin("m.ptr[6]"~op~"=b.m.ptr[6]; m.ptr[7]"~op~"=b.m.ptr[7]; m.ptr[8]"~op~"=b.m.ptr[8];");
1472 return this;
1475 auto opBinary(string op) (in Float b) const if (op == "*" || op == "/") {
1476 pragma(inline, true);
1477 m3 res = void;
1478 mixin("res.m.ptr[0] = m.ptr[0]"~op~"b;");
1479 mixin("res.m.ptr[1] = m.ptr[1]"~op~"b;");
1480 mixin("res.m.ptr[2] = m.ptr[2]"~op~"b;");
1481 mixin("res.m.ptr[3] = m.ptr[3]"~op~"b;");
1482 mixin("res.m.ptr[4] = m.ptr[4]"~op~"b;");
1483 mixin("res.m.ptr[5] = m.ptr[5]"~op~"b;");
1484 mixin("res.m.ptr[6] = m.ptr[6]"~op~"b;");
1485 mixin("res.m.ptr[7] = m.ptr[7]"~op~"b;");
1486 mixin("res.m.ptr[8] = m.ptr[8]"~op~"b;");
1487 return res;
1490 auto opBinaryRight(string op) (in Float b) const if (op == "*" || op == "/") {
1491 pragma(inline, true);
1492 m3 res = void;
1493 mixin("res.m.ptr[0] = m.ptr[0]"~op~"b;");
1494 mixin("res.m.ptr[1] = m.ptr[1]"~op~"b;");
1495 mixin("res.m.ptr[2] = m.ptr[2]"~op~"b;");
1496 mixin("res.m.ptr[3] = m.ptr[3]"~op~"b;");
1497 mixin("res.m.ptr[4] = m.ptr[4]"~op~"b;");
1498 mixin("res.m.ptr[5] = m.ptr[5]"~op~"b;");
1499 mixin("res.m.ptr[6] = m.ptr[6]"~op~"b;");
1500 mixin("res.m.ptr[7] = m.ptr[7]"~op~"b;");
1501 mixin("res.m.ptr[8] = m.ptr[8]"~op~"b;");
1502 return res;
1505 ref auto opOpAssign(string op) (in Float b) if (op == "*" || op == "/") {
1506 pragma(inline, true);
1507 mixin("m.ptr[0]"~op~"=b; m.ptr[1]"~op~"=b; m.ptr[2]"~op~"=b;");
1508 mixin("m.ptr[3]"~op~"=b; m.ptr[4]"~op~"=b; m.ptr[5]"~op~"=b;");
1509 mixin("m.ptr[6]"~op~"=b; m.ptr[7]"~op~"=b; m.ptr[8]"~op~"=b;");
1510 return this;
1513 static if (TwoD) auto opBinary(string op:"*") (in auto ref v2 v) const {
1514 pragma(inline, true);
1515 return v2(
1516 v.x*m.ptr[3*0+0]+v.y*m.ptr[3*1+0]+m.ptr[3*2+0],
1517 v.x*m.ptr[3*0+1]+v.y*m.ptr[3*1+1]+m.ptr[3*2+1],
1521 static if (ThreeD) auto opBinary(string op:"*") (in auto ref v3 v) const {
1522 pragma(inline, true);
1523 return v3(
1524 v.x*m.ptr[3*0+0]+v.y*m.ptr[3*1+0]+v.z*m.ptr[3*2+0],
1525 v.x*m.ptr[3*0+1]+v.y*m.ptr[3*1+1]+v.z*m.ptr[3*2+1],
1526 v.x*m.ptr[3*0+2]+v.y*m.ptr[3*1+2]+v.z*m.ptr[3*2+2],
1530 static if (TwoD) auto opBinaryRight(string op:"*") (in auto ref v2 v) const { pragma(inline, true); return this*v; }
1531 static if (ThreeD) auto opBinaryRight(string op:"*") (in auto ref v3 v) const { pragma(inline, true); return this*v; }
1533 auto opBinary(string op:"*") (in auto ref m3 b) const {
1534 //pragma(inline, true);
1535 m3 res = void;
1536 res.m.ptr[3*0+0] = m.ptr[3*0+0]*b.m.ptr[3*0+0]+m.ptr[3*0+1]*b.m.ptr[3*1+0]+m.ptr[3*0+2]*b.m.ptr[3*2+0];
1537 res.m.ptr[3*0+1] = m.ptr[3*0+0]*b.m.ptr[3*0+1]+m.ptr[3*0+1]*b.m.ptr[3*1+1]+m.ptr[3*0+2]*b.m.ptr[3*2+1];
1538 res.m.ptr[3*0+2] = m.ptr[3*0+0]*b.m.ptr[3*0+2]+m.ptr[3*0+1]*b.m.ptr[3*1+2]+m.ptr[3*0+2]*b.m.ptr[3*2+2];
1539 res.m.ptr[3*1+0] = m.ptr[3*1+0]*b.m.ptr[3*0+0]+m.ptr[3*1+1]*b.m.ptr[3*1+0]+m.ptr[3*1+2]*b.m.ptr[3*2+0];
1540 res.m.ptr[3*1+1] = m.ptr[3*1+0]*b.m.ptr[3*0+1]+m.ptr[3*1+1]*b.m.ptr[3*1+1]+m.ptr[3*1+2]*b.m.ptr[3*2+1];
1541 res.m.ptr[3*1+2] = m.ptr[3*1+0]*b.m.ptr[3*0+2]+m.ptr[3*1+1]*b.m.ptr[3*1+2]+m.ptr[3*1+2]*b.m.ptr[3*2+2];
1542 res.m.ptr[3*2+0] = m.ptr[3*2+0]*b.m.ptr[3*0+0]+m.ptr[3*2+1]*b.m.ptr[3*1+0]+m.ptr[3*2+2]*b.m.ptr[3*2+0];
1543 res.m.ptr[3*2+1] = m.ptr[3*2+0]*b.m.ptr[3*0+1]+m.ptr[3*2+1]*b.m.ptr[3*1+1]+m.ptr[3*2+2]*b.m.ptr[3*2+1];
1544 res.m.ptr[3*2+2] = m.ptr[3*2+0]*b.m.ptr[3*0+2]+m.ptr[3*2+1]*b.m.ptr[3*1+2]+m.ptr[3*2+2]*b.m.ptr[3*2+2];
1545 return res;
1548 // multiply vector by transposed matrix (TESTME!)
1549 static if (ThreeD) auto transmul() (in auto ref v3 v) const {
1550 pragma(inline, true);
1551 return v3(
1552 v.x*m.ptr[3*0+0]+v.y*m.ptr[3*0+1]+v.z*m.ptr[3*0+2],
1553 v.x*m.ptr[3*1+0]+v.y*m.ptr[3*1+1]+v.z*m.ptr[3*1+2],
1554 v.x*m.ptr[3*2+0]+v.y*m.ptr[3*2+1]+v.z*m.ptr[3*2+2],
1558 // sum of the diagonal components
1559 Float trace () const { pragma(inline, true); return m.ptr[3*0+0]+m.ptr[3*1+1]+m.ptr[3*2+2]; }
1561 // determinant
1562 Float det () const {
1563 pragma(inline, true);
1564 Float res = 0;
1565 res += m.ptr[3*0+0]*(m.ptr[3*1+1]*m.ptr[3*2+2]-m.ptr[3*2+1]*m.ptr[3*1+2]);
1566 res -= m.ptr[3*0+1]*(m.ptr[3*1+0]*m.ptr[3*2+2]-m.ptr[3*2+0]*m.ptr[3*1+2]);
1567 res += m.ptr[3*0+2]*(m.ptr[3*1+0]*m.ptr[3*2+1]-m.ptr[3*2+0]*m.ptr[3*1+1]);
1568 return res;
1571 auto transposed () const {
1572 pragma(inline, true);
1573 m3 res;
1574 res.m.ptr[3*0+0] = m.ptr[3*0+0];
1575 res.m.ptr[3*0+1] = m.ptr[3*1+0];
1576 res.m.ptr[3*0+2] = m.ptr[3*2+0];
1577 res.m.ptr[3*1+0] = m.ptr[3*0+1];
1578 res.m.ptr[3*1+1] = m.ptr[3*1+1];
1579 res.m.ptr[3*1+2] = m.ptr[3*2+1];
1580 res.m.ptr[3*2+0] = m.ptr[3*0+2];
1581 res.m.ptr[3*2+1] = m.ptr[3*1+2];
1582 res.m.ptr[3*2+2] = m.ptr[3*2+2];
1583 return res;
1586 auto inv () const {
1587 //pragma(inline, true);
1588 immutable mtp = this.transposed;
1589 m3 res = void;
1591 res.m.ptr[3*0+0] = mtp.m.ptr[3*1+1]*mtp.m.ptr[3*2+2]-mtp.m.ptr[3*2+1]*mtp.m.ptr[3*1+2];
1592 res.m.ptr[3*0+1] = mtp.m.ptr[3*1+0]*mtp.m.ptr[3*2+2]-mtp.m.ptr[3*2+0]*mtp.m.ptr[3*1+2];
1593 res.m.ptr[3*0+2] = mtp.m.ptr[3*1+0]*mtp.m.ptr[3*2+1]-mtp.m.ptr[3*2+0]*mtp.m.ptr[3*1+1];
1594 res.m.ptr[3*1+0] = mtp.m.ptr[3*0+1]*mtp.m.ptr[3*2+2]-mtp.m.ptr[3*2+1]*mtp.m.ptr[3*0+2];
1595 res.m.ptr[3*1+1] = mtp.m.ptr[3*0+0]*mtp.m.ptr[3*2+2]-mtp.m.ptr[3*2+0]*mtp.m.ptr[3*0+2];
1596 res.m.ptr[3*1+2] = mtp.m.ptr[3*0+0]*mtp.m.ptr[3*2+1]-mtp.m.ptr[3*2+0]*mtp.m.ptr[3*0+1];
1597 res.m.ptr[3*2+0] = mtp.m.ptr[3*0+1]*mtp.m.ptr[3*1+2]-mtp.m.ptr[3*1+1]*mtp.m.ptr[3*0+2];
1598 res.m.ptr[3*2+1] = mtp.m.ptr[3*0+0]*mtp.m.ptr[3*1+2]-mtp.m.ptr[3*1+0]*mtp.m.ptr[3*0+2];
1599 res.m.ptr[3*2+2] = mtp.m.ptr[3*0+0]*mtp.m.ptr[3*1+1]-mtp.m.ptr[3*1+0]*mtp.m.ptr[3*0+1];
1601 res.m.ptr[3*0+1] *= -1;
1602 res.m.ptr[3*1+0] *= -1;
1603 res.m.ptr[3*1+2] *= -1;
1604 res.m.ptr[3*2+1] *= -1;
1606 return res/this.det;
1609 static if (ThreeD) {
1610 ref m3 rotateX (Float angle) {
1611 mixin(ImportCoreMath!(Float, "cos", "sin"));
1612 alias A = this;
1614 // get the sine and cosine of the rotation angle
1615 immutable Float s = sin(angle);
1616 immutable Float c = cos(angle);
1618 // calculate the new values of the six affected matrix entries
1619 immutable Float temp01 = c*A[0, 1]+s*A[0, 2];
1620 immutable Float temp11 = c*A[1, 1]+s*A[1, 2];
1621 immutable Float temp21 = c*A[2, 1]+s*A[2, 2];
1622 immutable Float temp02 = c*A[0, 2]-s*A[0, 1];
1623 immutable Float temp12 = c*A[1, 2]-s*A[1, 1];
1624 immutable Float temp22 = c*A[2, 2]-s*A[2, 1];
1626 // put the results back into A
1627 A[0, 1] = temp01; A[0, 2] = temp02;
1628 A[1, 1] = temp11; A[1, 2] = temp12;
1629 A[2, 1] = temp21; A[2, 2] = temp22;
1631 return this;
1634 ref m3 rotateY (Float angle) {
1635 mixin(ImportCoreMath!(Float, "cos", "sin"));
1636 alias A = this;
1638 // get the sine and cosine of the rotation angle
1639 immutable Float s = sin(angle);
1640 immutable Float c = cos(angle);
1642 // calculate the new values of the six affected matrix entries
1643 immutable Float temp00 = c*A[0, 0]+s*A[0, 2];
1644 immutable Float temp10 = c*A[1, 0]+s*A[1, 2];
1645 immutable Float temp20 = c*A[2, 0]+s*A[2, 2];
1646 immutable Float temp02 = c*A[0, 2]-s*A[0, 0];
1647 immutable Float temp12 = c*A[1, 2]-s*A[1, 0];
1648 immutable Float temp22 = c*A[2, 2]-s*A[2, 0];
1650 // put the results back into XformToChange
1651 A[0, 0] = temp00; A[0, 2] = temp02;
1652 A[1, 0] = temp10; A[1, 2] = temp12;
1653 A[2, 0] = temp20; A[2, 2] = temp22;
1655 return this;
1658 ref m3 rotateZ (Float angle) {
1659 import core.stdc.math : cos, sin;
1660 alias A = this;
1662 // get the sine and cosine of the rotation angle
1663 immutable Float s = sin(angle);
1664 immutable Float c = cos(angle);
1666 // calculate the new values of the six affected matrix entries
1667 immutable Float temp00 = c*A[0, 0]+s*A[0, 1];
1668 immutable Float temp10 = c*A[1, 0]+s*A[1, 1];
1669 immutable Float temp20 = c*A[2, 0]+s*A[2, 1];
1670 immutable Float temp01 = c*A[0, 1]-s*A[0, 0];
1671 immutable Float temp11 = c*A[1, 1]-s*A[1, 0];
1672 immutable Float temp21 = c*A[2, 1]-s*A[2, 0];
1674 // put the results back into XformToChange
1675 A[0, 0] = temp00; A[0, 1] = temp01;
1676 A[1, 0] = temp10; A[1, 1] = temp11;
1677 A[2, 0] = temp20; A[2, 1] = temp21;
1679 return this;
1683 static:
1684 auto Identity () { pragma(inline, true); return m3(); }
1685 auto Zero () { pragma(inline, true); return m3(0); }
1687 static if (TwoD) {
1688 auto Rotate (in Float angle) {
1689 pragma(inline, true);
1690 mixin(ImportCoreMath!(Float, "cos", "sin"));
1691 immutable Float c = cos(angle);
1692 immutable Float s = sin(angle);
1693 m3 res;
1694 res.m.ptr[3*0+0] = c; res.m.ptr[3*0+1] = s;
1695 res.m.ptr[3*1+0] = -s; res.m.ptr[3*1+1] = c;
1696 return res;
1699 auto Scale (in Float sx, in Float sy) {
1700 pragma(inline, true);
1701 m3 res;
1702 res.m.ptr[3*0+0] = sx;
1703 res.m.ptr[3*1+1] = sy;
1704 return res;
1707 auto Scale() (in auto ref v2 sc) {
1708 pragma(inline, true);
1709 m3 res;
1710 res.m.ptr[3*0+0] = sc.x;
1711 res.m.ptr[3*1+1] = sc.y;
1712 return res;
1715 auto Translate (in Float dx, in Float dy) {
1716 pragma(inline, true);
1717 m3 res;
1718 res.m.ptr[3*2+0] = dx;
1719 res.m.ptr[3*2+1] = dy;
1720 return res;
1723 auto Translate() (in auto ref v2 v) {
1724 pragma(inline, true);
1725 m3 res;
1726 res.m.ptr[3*2+0] = v.x;
1727 res.m.ptr[3*2+1] = v.y;
1728 return res;
1732 static if (ThreeD) {
1733 // make rotation matrix from given angles
1734 static auto Rotate() (in auto ref v3 angles) {
1735 mixin(ImportCoreMath!(Float, "cos", "sin"));
1737 immutable Float cos_b = cos(angles[0]);
1738 immutable Float sin_b = sin(angles[0]);
1739 immutable Float cos_c = cos(angles[1]);
1740 immutable Float sin_c = sin(angles[1]);
1741 immutable Float cos_a = cos(angles[2]);
1742 immutable Float sin_a = sin(angles[2]);
1744 m3 M = void;
1746 // first matrix row
1747 M[0, 0] = cos_a*cos_c-sin_a*sin_b*sin_c;
1748 M[0, 1] = sin_a*cos_c+cos_a*sin_b*sin_c;
1749 M[0, 2] = cos_b*sin_c;
1751 // second matrix row
1752 M[1, 0] = -sin_a*cos_b;
1753 M[1, 1] = cos_a*cos_b;
1754 M[1, 2] = -sin_b;
1756 // third matrix row
1757 M[2, 0] = -cos_a*sin_c-sin_a*sin_b*cos_c;
1758 M[2, 1] = -sin_a*sin_c+cos_a*sin_b*cos_c;
1759 M[2, 2] = cos_b*cos_c;
1761 return M;
1764 static auto RotateX() (Float angle) {
1765 m3 res;
1766 res.rotateX(angle);
1767 return res;
1770 static auto RotateY() (Float angle) {
1771 m3 res;
1772 res.rotateY(angle);
1773 return res;
1776 static auto RotateZ() (Float angle) {
1777 m3 res;
1778 res.rotateZ(angle);
1779 return res;
1785 // ////////////////////////////////////////////////////////////////////////// //
1786 alias mat4 = Mat4!vec3;
1788 align(1) struct Mat4(VT) if (IsVectorDim!(VT, 3)) {
1789 align(1):
1790 public:
1791 alias Float = VT.Float;
1792 alias mat4 = typeof(this);
1793 alias vec3 = VecN!(3, Float);
1795 public:
1796 // OpenGL-compatible, row by row
1797 Float[4*4] mt = [
1798 1, 0, 0, 0,
1799 0, 1, 0, 0,
1800 0, 0, 1, 0,
1801 0, 0, 0, 1,
1804 nothrow @safe:
1805 string toString () const @trusted {
1806 import std.string : format;
1807 try {
1808 return "0:[%g,%g,%g,%g]\n4:[%g,%g,%g,%g]\n8:[%g,%g,%g,%g]\nc:[%g,%g,%g,%g]".format(
1809 mt.ptr[ 0], mt.ptr[ 1], mt.ptr[ 2], mt.ptr[ 3],
1810 mt.ptr[ 4], mt.ptr[ 5], mt.ptr[ 6], mt.ptr[ 7],
1811 mt.ptr[ 8], mt.ptr[ 9], mt.ptr[10], mt.ptr[11],
1812 mt.ptr[12], mt.ptr[13], mt.ptr[14], mt.ptr[15],
1814 } catch (Exception) {
1815 assert(0);
1819 Float opIndex (usize x, usize y) const @trusted @nogc { pragma(inline, true); return (x < 4 && y < 4 ? mt.ptr[y*4+x] : Float.nan); }
1820 void opIndexAssign (Float v, usize x, usize y) @trusted @nogc { pragma(inline, true); if (x < 4 && y < 4) mt.ptr[y*4+x] = v; }
1822 @nogc @trusted:
1823 this() (in Float[] vals...) { pragma(inline, true); if (vals.length >= 16) mt.ptr[0..16] = vals.ptr[0..16]; else { mt.ptr[0..16] = 0; mt.ptr[0..vals.length] = vals.ptr[0..vals.length]; } }
1824 this() (in auto ref mat4 m) { pragma(inline, true); mt.ptr[0..16] = m.mt.ptr[0..16]; }
1825 this() (in auto ref vec3 v) {
1826 //mt.ptr[0..16] = 0;
1827 mt.ptr[0*4+0] = v.x;
1828 mt.ptr[1*4+1] = v.y;
1829 mt.ptr[2*4+2] = v.z;
1830 //mt.ptr[3*4+3] = 1; // just in case
1833 static mat4 Zero () { pragma(inline, true); return mat4(0); }
1834 static mat4 Identity () { pragma(inline, true); /*mat4 res = Zero; res.mt.ptr[0*4+0] = res.mt.ptr[1*4+1] = res.mt.ptr[2*4+2] = res.mt.ptr[3*4+3] = 1; return res;*/ return mat4(); }
1836 // multiply current OpenGL matrix with this one
1837 // templated, to not compile it if we won't use it
1838 void glMultiply() () const {
1839 pragma(inline, true);
1840 import iv.glbinds;
1841 static if (is(Float == float)) {
1842 glMultMatrixf(mt.ptr);
1843 } else {
1844 glMultMatrixd(mt.ptr);
1848 static mat4 GLRetrieveAny() (uint mode) nothrow @trusted @nogc {
1849 pragma(inline, true);
1850 import iv.glbinds;
1851 mat4 res = void;
1852 static if (is(Float == float)) {
1853 glGetFloatv(mode, res.mt.ptr);
1854 } else {
1855 glGetDoublev(mode, res.mt.ptr);
1857 return res;
1860 static mat4 GLRetrieveModelView() () nothrow @trusted @nogc { pragma(inline, true); import iv.glbinds; return GLRetrieveAny(GL_MODELVIEW_MATRIX); }
1861 static mat4 GLRetrieveProjection() () nothrow @trusted @nogc { pragma(inline, true); import iv.glbinds; return GLRetrieveAny(GL_PROJECTION_MATRIX); }
1862 static mat4 GLRetrieveTexture() () nothrow @trusted @nogc { pragma(inline, true); import iv.glbinds; return GLRetrieveAny(GL_TEXTURE_MATRIX); }
1863 static mat4 GLRetrieveColor() () nothrow @trusted @nogc { pragma(inline, true); import iv.glbinds; return GLRetrieveAny(GL_COLOR_MATRIX); }
1865 void glRetrieveAny() (uint mode) nothrow @trusted @nogc {
1866 pragma(inline, true);
1867 import iv.glbinds;
1868 static if (is(Float == float)) {
1869 glGetFloatv(mode, mt.ptr);
1870 } else {
1871 glGetDoublev(mode, mt.ptr);
1875 void glRetrieveModelView() () nothrow @trusted @nogc { pragma(inline, true); import iv.glbinds; glRetrieveAny(GL_MODELVIEW_MATRIX); }
1876 void glRetrieveProjection() () nothrow @trusted @nogc { pragma(inline, true); import iv.glbinds; glRetrieveAny(GL_PROJECTION_MATRIX); }
1877 void glRetrieveTexture() () nothrow @trusted @nogc { pragma(inline, true); import iv.glbinds; glRetrieveAny(GL_TEXTURE_MATRIX); }
1878 void glRetrieveColor() () nothrow @trusted @nogc { pragma(inline, true); import iv.glbinds; glRetrieveAny(GL_COLOR_MATRIX); }
1880 @property bool isIdentity () const {
1881 pragma(inline, true); // can we?
1882 return
1883 mt.ptr[0] == 1 && mt.ptr[1] == 0 && mt.ptr[2] == 0 && mt.ptr[3] == 0 &&
1884 mt.ptr[4] == 0 && mt.ptr[5] == 1 && mt.ptr[6] == 0 && mt.ptr[7] == 0 &&
1885 mt.ptr[8] == 0 && mt.ptr[9] == 0 && mt.ptr[10] == 1 && mt.ptr[11] == 0 &&
1886 mt.ptr[12] == 0 && mt.ptr[13] == 0 && mt.ptr[14] == 0 && mt.ptr[15] == 1;
1889 @property inout(Float)* glGetVUnsafe () inout nothrow @trusted @nogc { pragma(inline, true); return cast(typeof(return))mt.ptr; }
1891 Float[4] getRow (int idx) const {
1892 Float[4] res = Float.nan;
1893 switch (idx) {
1894 case 0:
1895 res.ptr[0] = mt.ptr[0];
1896 res.ptr[1] = mt.ptr[4];
1897 res.ptr[2] = mt.ptr[8];
1898 res.ptr[3] = mt.ptr[12];
1899 break;
1900 case 1:
1901 res.ptr[0] = mt.ptr[1];
1902 res.ptr[1] = mt.ptr[5];
1903 res.ptr[2] = mt.ptr[9];
1904 res.ptr[3] = mt.ptr[13];
1905 break;
1906 case 2:
1907 res.ptr[0] = mt.ptr[2];
1908 res.ptr[1] = mt.ptr[6];
1909 res.ptr[2] = mt.ptr[10];
1910 res.ptr[3] = mt.ptr[14];
1911 break;
1912 case 3:
1913 res.ptr[0] = mt.ptr[3];
1914 res.ptr[1] = mt.ptr[7];
1915 res.ptr[2] = mt.ptr[11];
1916 res.ptr[3] = mt.ptr[15];
1917 break;
1918 default: break;
1920 return res;
1923 Float[4] getCol (int idx) const {
1924 Float[4] res = Float.nan;
1925 if (idx >= 0 && idx <= 3) res = mt.ptr[idx*4..idx*5];
1926 return res;
1929 // this is for camera matrices
1930 vec3 upVector () const { pragma(inline, true); return vec3(mt.ptr[1], mt.ptr[5], mt.ptr[9]); }
1931 vec3 rightVector () const { pragma(inline, true); return vec3(mt.ptr[0], mt.ptr[4], mt.ptr[8]); }
1932 vec3 forwardVector () const { pragma(inline, true); return vec3(mt.ptr[2], mt.ptr[6], mt.ptr[10]); }
1934 private enum SinCosImportMixin = q{
1935 static if (is(Float == float)) {
1936 import core.stdc.math : cos=cosf, sin=sinf;
1937 } else {
1938 import core.stdc.math : cos, sin;
1942 static mat4 RotateX() (Float angle) {
1943 mixin(SinCosImportMixin);
1944 auto res = mat4(0);
1945 res.mt.ptr[0*4+0] = cast(Float)1;
1946 res.mt.ptr[1*4+1] = cos(angle);
1947 res.mt.ptr[2*4+1] = -sin(angle);
1948 res.mt.ptr[1*4+2] = sin(angle);
1949 res.mt.ptr[2*4+2] = cos(angle);
1950 res.mt.ptr[3*4+3] = cast(Float)1;
1951 return res;
1954 static mat4 RotateY() (Float angle) {
1955 mixin(SinCosImportMixin);
1956 auto res = mat4(0);
1957 res.mt.ptr[0*4+0] = cos(angle);
1958 res.mt.ptr[2*4+0] = sin(angle);
1959 res.mt.ptr[1*4+1] = cast(Float)1;
1960 res.mt.ptr[0*4+2] = -sin(angle);
1961 res.mt.ptr[2*4+2] = cos(angle);
1962 res.mt.ptr[3*4+3] = cast(Float)1;
1963 return res;
1966 static mat4 RotateZ() (Float angle) {
1967 mixin(SinCosImportMixin);
1968 auto res = mat4(0);
1969 res.mt.ptr[0*4+0] = cos(angle);
1970 res.mt.ptr[1*4+0] = -sin(angle);
1971 res.mt.ptr[0*4+1] = sin(angle);
1972 res.mt.ptr[1*4+1] = cos(angle);
1973 res.mt.ptr[2*4+2] = cast(Float)1;
1974 res.mt.ptr[3*4+3] = cast(Float)1;
1975 return res;
1978 static mat4 Translate() (in auto ref vec3 v) {
1979 auto res = mat4(0);
1980 res.mt.ptr[0*4+0] = res.mt.ptr[1*4+1] = res.mt.ptr[2*4+2] = 1;
1981 res.mt.ptr[3*4+0] = v.x;
1982 res.mt.ptr[3*4+1] = v.y;
1983 res.mt.ptr[3*4+2] = v.z;
1984 res.mt.ptr[3*4+3] = 1;
1985 return res;
1988 static mat4 TranslateNeg() (in auto ref vec3 v) {
1989 auto res = mat4(0);
1990 res.mt.ptr[0*4+0] = res.mt.ptr[1*4+1] = res.mt.ptr[2*4+2] = 1;
1991 res.mt.ptr[3*4+0] = -v.x;
1992 res.mt.ptr[3*4+1] = -v.y;
1993 res.mt.ptr[3*4+2] = -v.z;
1994 res.mt.ptr[3*4+3] = 1;
1995 return res;
1998 static mat4 Scale() (in auto ref vec3 v) {
1999 auto res = mat4(0);
2000 res.mt.ptr[0*4+0] = v.x;
2001 res.mt.ptr[1*4+1] = v.y;
2002 res.mt.ptr[2*4+2] = v.z;
2003 res.mt.ptr[3*4+3] = 1;
2004 return res;
2007 static mat4 Rotate() (in auto ref vec3 v) {
2008 auto mx = mat4.RotateX(v.x);
2009 auto my = mat4.RotateY(v.y);
2010 auto mz = mat4.RotateZ(v.z);
2011 return mz*my*mx;
2014 static mat4 RotateDeg() (in auto ref vec3 v) {
2015 auto mx = mat4.RotateX(v.x.deg2rad);
2016 auto my = mat4.RotateY(v.y.deg2rad);
2017 auto mz = mat4.RotateZ(v.z.deg2rad);
2018 return mz*my*mx;
2021 // for camera; x is pitch (up/down); y is yaw (left/right); z is roll (tilt)
2022 static mat4 RotateZXY() (in auto ref vec3 v) {
2023 auto mx = mat4.RotateX(v.x);
2024 auto my = mat4.RotateY(v.y);
2025 auto mz = mat4.RotateZ(v.z);
2026 return mz*mx*my;
2029 // for camera; x is pitch (up/down); y is yaw (left/right); z is roll (tilt)
2030 static mat4 RotateZXYDeg() (in auto ref vec3 v) {
2031 auto mx = mat4.RotateX(v.x.deg2rad);
2032 auto my = mat4.RotateY(v.y.deg2rad);
2033 auto mz = mat4.RotateZ(v.z.deg2rad);
2034 return mz*mx*my;
2037 // same as `glFrustum()`
2038 static mat4 Frustum() (Float left, Float right, Float bottom, Float top, Float nearVal, Float farVal) nothrow @trusted @nogc {
2039 auto res = mat4(0);
2040 res.mt.ptr[0] = 2*nearVal/(right-left);
2041 res.mt.ptr[5] = 2*nearVal/(top-bottom);
2042 res.mt.ptr[8] = (right+left)/(right-left);
2043 res.mt.ptr[9] = (top+bottom)/(top-bottom);
2044 res.mt.ptr[10] = -(farVal+nearVal)/(farVal-nearVal);
2045 res.mt.ptr[11] = -1;
2046 res.mt.ptr[14] = -(2*farVal*nearVal)/(farVal-nearVal);
2047 res.mt.ptr[15] = 0;
2048 return res;
2051 // same as `glOrtho()`
2052 static mat4 Ortho() (Float left, Float right, Float bottom, Float top, Float nearVal, Float farVal) nothrow @trusted @nogc {
2053 auto res = mat4(0);
2054 res.mt.ptr[0] = 2/(right-left);
2055 res.mt.ptr[5] = 2/(top-bottom);
2056 res.mt.ptr[10] = -2/(farVal-nearVal);
2057 res.mt.ptr[12] = -(right+left)/(right-left);
2058 res.mt.ptr[13] = -(top+bottom)/(top-bottom);
2059 res.mt.ptr[14] = -(farVal+nearVal)/(farVal-nearVal);
2060 return res;
2063 // same as `gluPerspective()`
2064 // sets the frustum to perspective mode
2065 // fovY - Field of vision in degrees in the y direction
2066 // aspect - Aspect ratio of the viewport
2067 // zNear - The near clipping distance
2068 // zFar - The far clipping distance
2069 static mat4 Perspective() (Float fovY, Float aspect, Float zNear, Float zFar) nothrow @trusted @nogc {
2070 import std.math : PI;
2071 mixin(ImportCoreMath!(Float, "tan"));
2072 immutable Float fH = cast(Float)(tan(fovY/360*PI)*zNear);
2073 immutable Float fW = cast(Float)(fH*aspect);
2074 return Frustum(-fW, fW, -fH, fH, zNear, zFar);
2077 public:
2078 static mat4 LookAtFucked() (in auto ref vec3 eye, in auto ref vec3 center, in auto ref vec3 up) {
2079 // compute vector `N = EP-VRP` and normalize `N`
2080 vec3 n = eye-center;
2081 n.normalize;
2083 // compute vector `V = UP-VRP`
2084 // make vector `V` orthogonal to `N` and normalize `V`
2085 vec3 v = up-center;
2086 immutable dp = v.dot(n); //dp = (float)V3Dot(&v,&n);
2087 //v.x -= dp * n.x; v.y -= dp * n.y; v.z -= dp * n.z;
2088 v -= n*dp;
2089 v.normalize;
2091 // compute vector `U = V x N` (cross product)
2092 immutable vec3 u = v.cross(n);
2094 // write the vectors `U`, `V`, and `N` as the first three rows of first, second, and third columns of transformation matrix
2095 mat4 m = void;
2097 m.mt.ptr[0*4+0] = u.x;
2098 m.mt.ptr[1*4+0] = u.y;
2099 m.mt.ptr[2*4+0] = u.z;
2100 //m.mt.ptr[3*4+0] = 0;
2102 m.mt.ptr[0*4+1] = v.x;
2103 m.mt.ptr[1*4+1] = v.y;
2104 m.mt.ptr[2*4+1] = v.z;
2105 //m.mt.ptr[3*4+1] = 0;
2107 m.mt.ptr[0*4+2] = n.x;
2108 m.mt.ptr[1*4+2] = n.y;
2109 m.mt.ptr[2*4+2] = n.z;
2110 //m.mt.ptr[3*4+2] = 0;
2112 // compute the fourth row of transformation matrix to include the translation of `VRP` to the origin
2113 m.mt.ptr[3*4+0] = -u.x*center.x-u.y*center.y-u.z*center.z;
2114 m.mt.ptr[3*4+1] = -v.x*center.x-v.y*center.y-v.z*center.z;
2115 m.mt.ptr[3*4+2] = -n.x*center.x-n.y*center.y-n.z*center.z;
2116 m.mt.ptr[3*4+3] = 1;
2118 foreach (ref Float f; m.mt[]) {
2119 mixin(ImportCoreMath!(Float, "fabs"));
2120 if (fabs(f) < EPSILON!Float) f = 0;
2123 return m;
2126 // does `gluLookAt()`
2127 mat4 lookAt() (in auto ref vec3 eye, in auto ref vec3 center, in auto ref vec3 up) const {
2128 mixin(ImportCoreMath!(Float, "sqrt"));
2130 mat4 m = void;
2131 Float[3] x = void, y = void, z = void;
2132 Float mag;
2133 // make rotation matrix
2134 // Z vector
2135 z.ptr[0] = eye.x-center.x;
2136 z.ptr[1] = eye.y-center.y;
2137 z.ptr[2] = eye.z-center.z;
2138 mag = sqrt(z.ptr[0]*z.ptr[0]+z.ptr[1]*z.ptr[1]+z.ptr[2]*z.ptr[2]);
2139 if (mag != 0) {
2140 z.ptr[0] /= mag;
2141 z.ptr[1] /= mag;
2142 z.ptr[2] /= mag;
2144 // Y vector
2145 y.ptr[0] = up.x;
2146 y.ptr[1] = up.y;
2147 y.ptr[2] = up.z;
2148 // X vector = Y cross Z
2149 x.ptr[0] = y.ptr[1]*z.ptr[2]-y.ptr[2]*z.ptr[1];
2150 x.ptr[1] = -y.ptr[0]*z.ptr[2]+y.ptr[2]*z.ptr[0];
2151 x.ptr[2] = y.ptr[0]*z.ptr[1]-y.ptr[1]*z.ptr[0];
2152 // Recompute Y = Z cross X
2153 y.ptr[0] = z.ptr[1]*x.ptr[2]-z.ptr[2]*x.ptr[1];
2154 y.ptr[1] = -z.ptr[0]*x.ptr[2]+z.ptr[2]*x.ptr[0];
2155 y.ptr[2] = z.ptr[0]*x.ptr[1]-z.ptr[1]*x.ptr[0];
2157 /* cross product gives area of parallelogram, which is < 1.0 for
2158 * non-perpendicular unit-length vectors; so normalize x, y here
2160 mag = sqrt(x.ptr[0]*x.ptr[0]+x.ptr[1]*x.ptr[1]+x.ptr[2]*x.ptr[2]);
2161 if (mag != 0) {
2162 x.ptr[0] /= mag;
2163 x.ptr[1] /= mag;
2164 x.ptr[2] /= mag;
2167 mag = sqrt(y.ptr[0]*y.ptr[0]+y.ptr[1]*y.ptr[1]+y.ptr[2]*y.ptr[2]);
2168 if (mag != 0) {
2169 y.ptr[0] /= mag;
2170 y.ptr[1] /= mag;
2171 y.ptr[2] /= mag;
2174 m.mt.ptr[0*4+0] = x.ptr[0];
2175 m.mt.ptr[1*4+0] = x.ptr[1];
2176 m.mt.ptr[2*4+0] = x.ptr[2];
2177 m.mt.ptr[3*4+0] = 0;
2178 m.mt.ptr[0*4+1] = y.ptr[0];
2179 m.mt.ptr[1*4+1] = y.ptr[1];
2180 m.mt.ptr[2*4+1] = y.ptr[2];
2181 m.mt.ptr[3*4+1] = 0;
2182 m.mt.ptr[0*4+2] = z.ptr[0];
2183 m.mt.ptr[1*4+2] = z.ptr[1];
2184 m.mt.ptr[2*4+2] = z.ptr[2];
2185 m.mt.ptr[3*4+2] = 0;
2186 m.mt.ptr[0*4+3] = 0;
2187 m.mt.ptr[1*4+3] = 0;
2188 m.mt.ptr[2*4+3] = 0;
2189 m.mt.ptr[3*4+3] = 1;
2191 // move, and translate Eye to Origin
2192 return this*m*Translate(-eye);
2195 // rotate matrix to face along the target direction
2196 // this function will clear the previous rotation and scale, but it will keep the previous translation
2197 // it is for rotating object to look at the target, NOT for camera
2198 ref mat4 lookingAt() (in auto ref vec3 target) {
2199 mixin(ImportCoreMath!(Float, "fabs"));
2200 vec3 position = vec3(mt.ptr[12], mt.ptr[13], mt.ptr[14]);
2201 vec3 forward = (target-position).normalized;
2202 vec3 up;
2203 if (fabs(forward.x) < EPSILON!Float && fabs(forward.z) < EPSILON!Float) {
2204 up.z = (forward.y > 0 ? -1 : 1);
2205 } else {
2206 up.y = 1;
2208 vec3 left = up.cross(forward).normalized;
2209 up = forward.cross(left).normalized; //k8: `normalized` was commented out; why?
2210 mt.ptr[0*4+0] = left.x;
2211 mt.ptr[0*4+1] = left.y;
2212 mt.ptr[0*4+2] = left.z;
2213 mt.ptr[1*4+0] = up.x;
2214 mt.ptr[1*4+1] = up.y;
2215 mt.ptr[1*4+2] = up.z;
2216 mt.ptr[2*4+0] = forward.x;
2217 mt.ptr[2*4+1] = forward.y;
2218 mt.ptr[2*4+2] = forward.z;
2219 return this;
2222 ref mat4 lookingAt() (in auto ref vec3 target, in auto ref vec3 upVec) {
2223 vec3 position = vec3(mt.ptr[12], mt.ptr[13], mt.ptr[14]);
2224 vec3 forward = (target-position).normalized;
2225 vec3 left = upVec.cross(forward).normalized;
2226 vec3 up = forward.cross(left).normalized;
2227 mt.ptr[0*4+0] = left.x;
2228 mt.ptr[0*4+1] = left.y;
2229 mt.ptr[0*4+2] = left.z;
2230 mt.ptr[1*4+0] = up.x;
2231 mt.ptr[1*4+1] = up.y;
2232 mt.ptr[1*4+2] = up.z;
2233 mt.ptr[2*4+0] = forward.x;
2234 mt.ptr[2*4+1] = forward.y;
2235 mt.ptr[2*4+2] = forward.z;
2236 return this;
2239 mat4 lookAt() (in auto ref vec3 target) { pragma(inline, true); auto res = mat4(this); return this.lookingAt(target); }
2240 mat4 lookAt() (in auto ref vec3 target, in auto ref vec3 upVec) { pragma(inline, true); auto res = mat4(this); return this.lookingAt(target, upVec); }
2242 ref mat4 rotate() (Float angle, in auto ref vec3 axis) {
2243 mixin(SinCosImportMixin);
2244 angle = deg2rad(angle);
2245 immutable Float c = cos(angle);
2246 immutable Float s = sin(angle);
2247 immutable Float c1 = 1-c;
2248 immutable Float m0 = mt.ptr[0], m4 = mt.ptr[4], m8 = mt.ptr[8], m12 = mt.ptr[12];
2249 immutable Float m1 = mt.ptr[1], m5 = mt.ptr[5], m9 = mt.ptr[9], m13 = mt.ptr[13];
2250 immutable Float m2 = mt.ptr[2], m6 = mt.ptr[6], m10 = mt.ptr[10], m14 = mt.ptr[14];
2252 // build rotation matrix
2253 immutable Float r0 = axis.x*axis.x*c1+c;
2254 immutable Float r1 = axis.x*axis.y*c1+axis.z*s;
2255 immutable Float r2 = axis.x*axis.z*c1-axis.y*s;
2256 immutable Float r4 = axis.x*axis.y*c1-axis.z*s;
2257 immutable Float r5 = axis.y*axis.y*c1+c;
2258 immutable Float r6 = axis.y*axis.z*c1+axis.x*s;
2259 immutable Float r8 = axis.x*axis.z*c1+axis.y*s;
2260 immutable Float r9 = axis.y*axis.z*c1-axis.x*s;
2261 immutable Float r10= axis.z*axis.z*c1+c;
2263 // multiply rotation matrix
2264 mt.ptr[0] = r0*m0+r4*m1+r8*m2;
2265 mt.ptr[1] = r1*m0+r5*m1+r9*m2;
2266 mt.ptr[2] = r2*m0+r6*m1+r10*m2;
2267 mt.ptr[4] = r0*m4+r4*m5+r8*m6;
2268 mt.ptr[5] = r1*m4+r5*m5+r9*m6;
2269 mt.ptr[6] = r2*m4+r6*m5+r10*m6;
2270 mt.ptr[8] = r0*m8+r4*m9+r8*m10;
2271 mt.ptr[9] = r1*m8+r5*m9+r9*m10;
2272 mt.ptr[10] = r2*m8+r6*m9+r10*m10;
2273 mt.ptr[12] = r0*m12+r4*m13+r8*m14;
2274 mt.ptr[13] = r1*m12+r5*m13+r9*m14;
2275 mt.ptr[14] = r2*m12+r6*m13+r10*m14;
2277 return this;
2280 ref mat4 rotateX() (Float angle) {
2281 mixin(SinCosImportMixin);
2282 angle = deg2rad(angle);
2283 immutable Float c = cos(angle);
2284 immutable Float s = sin(angle);
2285 immutable Float m1 = mt.ptr[1], m2 = mt.ptr[2];
2286 immutable Float m5 = mt.ptr[5], m6 = mt.ptr[6];
2287 immutable Float m9 = mt.ptr[9], m10 = mt.ptr[10];
2288 immutable Float m13 = mt.ptr[13], m14 = mt.ptr[14];
2290 mt.ptr[1] = m1*c+m2*-s;
2291 mt.ptr[2] = m1*s+m2*c;
2292 mt.ptr[5] = m5*c+m6*-s;
2293 mt.ptr[6] = m5*s+m6*c;
2294 mt.ptr[9] = m9*c+m10*-s;
2295 mt.ptr[10]= m9*s+m10*c;
2296 mt.ptr[13]= m13*c+m14*-s;
2297 mt.ptr[14]= m13*s+m14*c;
2299 return this;
2302 ref mat4 rotateY() (Float angle) {
2303 mixin(SinCosImportMixin);
2304 angle = deg2rad(angle);
2305 immutable Float c = cos(angle);
2306 immutable Float s = sin(angle);
2307 immutable Float m0 = mt.ptr[0], m2 = mt.ptr[2];
2308 immutable Float m4 = mt.ptr[4], m6 = mt.ptr[6];
2309 immutable Float m8 = mt.ptr[8], m10 = mt.ptr[10];
2310 immutable Float m12 = mt.ptr[12], m14 = mt.ptr[14];
2312 mt.ptr[0] = m0*c+m2*s;
2313 mt.ptr[2] = m0*-s+m2*c;
2314 mt.ptr[4] = m4*c+m6*s;
2315 mt.ptr[6] = m4*-s+m6*c;
2316 mt.ptr[8] = m8*c+m10*s;
2317 mt.ptr[10]= m8*-s+m10*c;
2318 mt.ptr[12]= m12*c+m14*s;
2319 mt.ptr[14]= m12*-s+m14*c;
2321 return this;
2324 ref mat4 rotateZ() (Float angle) {
2325 mixin(SinCosImportMixin);
2326 angle = deg2rad(angle);
2327 immutable Float c = cos(angle);
2328 immutable Float s = sin(angle);
2329 immutable Float m0 = mt.ptr[0], m1 = mt.ptr[1];
2330 immutable Float m4 = mt.ptr[4], m5 = mt.ptr[5];
2331 immutable Float m8 = mt.ptr[8], m9 = mt.ptr[9];
2332 immutable Float m12 = mt.ptr[12], m13 = mt.ptr[13];
2334 mt.ptr[0] = m0*c+m1*-s;
2335 mt.ptr[1] = m0*s+m1*c;
2336 mt.ptr[4] = m4*c+m5*-s;
2337 mt.ptr[5] = m4*s+m5*c;
2338 mt.ptr[8] = m8*c+m9*-s;
2339 mt.ptr[9] = m8*s+m9*c;
2340 mt.ptr[12]= m12*c+m13*-s;
2341 mt.ptr[13]= m12*s+m13*c;
2343 return this;
2346 //k8: wtf is this?!
2347 ref mat4 translate() (in auto ref vec3 v) {
2348 mt.ptr[0] += mt.ptr[3]*v.x; mt.ptr[4] += mt.ptr[7]*v.x; mt.ptr[8] += mt.ptr[11]*v.x; mt.ptr[12] += mt.ptr[15]*v.x;
2349 mt.ptr[1] += mt.ptr[3]*v.y; mt.ptr[5] += mt.ptr[7]*v.y; mt.ptr[9] += mt.ptr[11]*v.y; mt.ptr[13] += mt.ptr[15]*v.y;
2350 mt.ptr[2] += mt.ptr[3]*v.z; mt.ptr[6] += mt.ptr[7]*v.z; mt.ptr[10] += mt.ptr[11]*v.z; mt.ptr[14] += mt.ptr[15]*v.z;
2351 return this;
2354 ref mat4 translateNeg() (in auto ref vec3 v) {
2355 mt.ptr[0] -= mt.ptr[3]*v.x; mt.ptr[4] -= mt.ptr[7]*v.x; mt.ptr[8] -= mt.ptr[11]*v.x; mt.ptr[12] -= mt.ptr[15]*v.x;
2356 mt.ptr[1] -= mt.ptr[3]*v.y; mt.ptr[5] -= mt.ptr[7]*v.y; mt.ptr[9] -= mt.ptr[11]*v.y; mt.ptr[13] -= mt.ptr[15]*v.y;
2357 mt.ptr[2] -= mt.ptr[3]*v.z; mt.ptr[6] -= mt.ptr[7]*v.z; mt.ptr[10] -= mt.ptr[11]*v.z; mt.ptr[14] -= mt.ptr[15]*v.z;
2358 return this;
2362 ref mat4 translate() (in auto ref vec3 v) {
2363 mt.ptr[12] += v.x;
2364 mt.ptr[13] += v.y;
2365 mt.ptr[14] += v.z;
2366 return this;
2369 ref mat4 translateNeg() (in auto ref vec3 v) {
2370 mt.ptr[12] -= v.x;
2371 mt.ptr[13] -= v.y;
2372 mt.ptr[14] -= v.z;
2373 return this;
2377 //k8: wtf is this?!
2378 ref mat4 scale() (in auto ref vec3 v) {
2379 mt.ptr[0] *= v.x; mt.ptr[4] *= v.x; mt.ptr[8] *= v.x; mt.ptr[12] *= v.x;
2380 mt.ptr[1] *= v.y; mt.ptr[5] *= v.y; mt.ptr[9] *= v.y; mt.ptr[13] *= v.y;
2381 mt.ptr[2] *= v.z; mt.ptr[6] *= v.z; mt.ptr[10] *= v.z; mt.ptr[14] *= v.z;
2382 return this;
2385 mat4 rotated() (Float angle, in auto ref vec3 axis) const { pragma(inline, true); auto res = mat4(this); return res.rotate(angle, axis); }
2386 mat4 rotatedX() (Float angle) const { pragma(inline, true); auto res = mat4(this); return res.rotateX(angle); }
2387 mat4 rotatedY() (Float angle) const { pragma(inline, true); auto res = mat4(this); return res.rotateY(angle); }
2388 mat4 rotatedZ() (Float angle) const { pragma(inline, true); auto res = mat4(this); return res.rotateZ(angle); }
2389 mat4 translated() (in auto ref vec3 v) const { pragma(inline, true); auto res = mat4(this); return res.translate(v); }
2390 mat4 translatedNeg() (in auto ref vec3 v) const { pragma(inline, true); auto res = mat4(this); return res.translateNeg(v); }
2391 mat4 scaled() (in auto ref vec3 v) const { pragma(inline, true); auto res = mat4(this); return res.scale(v); }
2393 // retrieve angles in degree from rotation matrix, M = Rx*Ry*Rz, in degrees
2394 // Rx: rotation about X-axis, pitch
2395 // Ry: rotation about Y-axis, yaw (heading)
2396 // Rz: rotation about Z-axis, roll
2397 vec3 getAnglesDeg () const {
2398 mixin(ImportCoreMath!(Float, "asin", "atan2"));
2399 Float pitch = void, roll = void;
2400 Float yaw = rad2deg(asin(mt.ptr[8]));
2401 if (mt.ptr[10] < 0) {
2402 if (yaw >= 0) yaw = 180-yaw; else yaw = -180-yaw;
2404 if (mt.ptr[0] > -EPSILON!Float && mt.ptr[0] < EPSILON!Float) {
2405 roll = 0;
2406 pitch = rad2deg(atan2(mt.ptr[1], mt.ptr[5]));
2407 } else {
2408 roll = rad2deg(atan2(-mt.ptr[4], mt.ptr[0]));
2409 pitch = rad2deg(atan2(-mt.ptr[9], mt.ptr[10]));
2411 return vec3(pitch, yaw, roll);
2414 // retrieve angles in degree from rotation matrix, M = Rx*Ry*Rz, in radians
2415 // Rx: rotation about X-axis, pitch
2416 // Ry: rotation about Y-axis, yaw (heading)
2417 // Rz: rotation about Z-axis, roll
2418 vec3 getAngles () const {
2419 mixin(ImportCoreMath!(Float, "asin", "atan2"));
2420 Float pitch = void, roll = void;
2421 Float yaw = asin(mt.ptr[8]);
2422 if (mt.ptr[10] < 0) {
2423 if (yaw >= 0) yaw = 180-yaw; else yaw = -180-yaw;
2425 if (mt.ptr[0] > -EPSILON!Float && mt.ptr[0] < EPSILON!Float) {
2426 roll = 0;
2427 pitch = atan2(mt.ptr[1], mt.ptr[5]);
2428 } else {
2429 roll = atan2(-mt.ptr[4], mt.ptr[0]);
2430 pitch = atan2(-mt.ptr[9], mt.ptr[10]);
2432 return vec3(pitch, yaw, roll);
2435 vec3 opBinary(string op:"*") (in auto ref vec3 v) const {
2436 //pragma(inline, true);
2437 return vec3(
2438 mt.ptr[0*4+0]*v.x+mt.ptr[1*4+0]*v.y+mt.ptr[2*4+0]*v.z+mt.ptr[3*4+0],
2439 mt.ptr[0*4+1]*v.x+mt.ptr[1*4+1]*v.y+mt.ptr[2*4+1]*v.z+mt.ptr[3*4+1],
2440 mt.ptr[0*4+2]*v.x+mt.ptr[1*4+2]*v.y+mt.ptr[2*4+2]*v.z+mt.ptr[3*4+2]);
2443 vec3 opBinaryRight(string op:"*") (in auto ref vec3 v) const {
2444 //pragma(inline, true);
2445 return vec3(
2446 mt.ptr[0*4+0]*v.x+mt.ptr[0*4+1]*v.y+mt.ptr[0*4+2]*v.z+mt.ptr[0*4+3],
2447 mt.ptr[1*4+0]*v.x+mt.ptr[1*4+1]*v.y+mt.ptr[1*4+2]*v.z+mt.ptr[1*4+3],
2448 mt.ptr[2*4+0]*v.x+mt.ptr[2*4+1]*v.y+mt.ptr[2*4+2]*v.z+mt.ptr[2*4+3]);
2451 mat4 opBinary(string op:"*") (in auto ref mat4 m) const {
2452 //pragma(inline, true);
2453 return mat4(
2454 mt.ptr[0]*m.mt.ptr[0] +mt.ptr[4]*m.mt.ptr[1] +mt.ptr[8]*m.mt.ptr[2] +mt.ptr[12]*m.mt.ptr[3], mt.ptr[1]*m.mt.ptr[0] +mt.ptr[5]*m.mt.ptr[1] +mt.ptr[9]*m.mt.ptr[2] +mt.ptr[13]*m.mt.ptr[3], mt.ptr[2]*m.mt.ptr[0] +mt.ptr[6]*m.mt.ptr[1] +mt.ptr[10]*m.mt.ptr[2] +mt.ptr[14]*m.mt.ptr[3], mt.ptr[3]*m.mt.ptr[0] +mt.ptr[7]*m.mt.ptr[1] +mt.ptr[11]*m.mt.ptr[2] +mt.ptr[15]*m.mt.ptr[3],
2455 mt.ptr[0]*m.mt.ptr[4] +mt.ptr[4]*m.mt.ptr[5] +mt.ptr[8]*m.mt.ptr[6] +mt.ptr[12]*m.mt.ptr[7], mt.ptr[1]*m.mt.ptr[4] +mt.ptr[5]*m.mt.ptr[5] +mt.ptr[9]*m.mt.ptr[6] +mt.ptr[13]*m.mt.ptr[7], mt.ptr[2]*m.mt.ptr[4] +mt.ptr[6]*m.mt.ptr[5] +mt.ptr[10]*m.mt.ptr[6] +mt.ptr[14]*m.mt.ptr[7], mt.ptr[3]*m.mt.ptr[4] +mt.ptr[7]*m.mt.ptr[5] +mt.ptr[11]*m.mt.ptr[6] +mt.ptr[15]*m.mt.ptr[7],
2456 mt.ptr[0]*m.mt.ptr[8] +mt.ptr[4]*m.mt.ptr[9] +mt.ptr[8]*m.mt.ptr[10]+mt.ptr[12]*m.mt.ptr[11],mt.ptr[1]*m.mt.ptr[8] +mt.ptr[5]*m.mt.ptr[9] +mt.ptr[9]*m.mt.ptr[10]+mt.ptr[13]*m.mt.ptr[11],mt.ptr[2]*m.mt.ptr[8] +mt.ptr[6]*m.mt.ptr[9] +mt.ptr[10]*m.mt.ptr[10]+mt.ptr[14]*m.mt.ptr[11],mt.ptr[3]*m.mt.ptr[8] +mt.ptr[7]*m.mt.ptr[9] +mt.ptr[11]*m.mt.ptr[10]+mt.ptr[15]*m.mt.ptr[11],
2457 mt.ptr[0]*m.mt.ptr[12]+mt.ptr[4]*m.mt.ptr[13]+mt.ptr[8]*m.mt.ptr[14]+mt.ptr[12]*m.mt.ptr[15],mt.ptr[1]*m.mt.ptr[12]+mt.ptr[5]*m.mt.ptr[13]+mt.ptr[9]*m.mt.ptr[14]+mt.ptr[13]*m.mt.ptr[15],mt.ptr[2]*m.mt.ptr[12]+mt.ptr[6]*m.mt.ptr[13]+mt.ptr[10]*m.mt.ptr[14]+mt.ptr[14]*m.mt.ptr[15],mt.ptr[3]*m.mt.ptr[12]+mt.ptr[7]*m.mt.ptr[13]+mt.ptr[11]*m.mt.ptr[14]+mt.ptr[15]*m.mt.ptr[15],
2461 mat4 opBinary(string op:"+") (in auto ref mat4 m) const {
2462 auto res = mat4(this);
2463 res.mt[] += m.mt[];
2464 return res;
2467 mat4 opBinary(string op:"*") (Float a) const {
2468 auto res = mat4(this);
2469 res.mt[] *= a;
2470 return res;
2473 mat4 opBinary(string op:"/") (Float a) const {
2474 mixin(ImportCoreMath!(Float, "fabs"));
2475 auto res = mat4(this);
2476 if (fabs(a) >= FLTEPS) {
2477 a = cast(Float)1/a;
2478 res.mt[] *= a;
2479 } else {
2480 res.mt[] = 0;
2482 return res;
2485 ref vec2 opOpAssign(string op:"*") (in auto ref mat4 m) {
2486 mat4 res;
2487 foreach (immutable i; 0..4) {
2488 foreach (immutable j; 0..4) {
2489 foreach (immutable k; 0..4) {
2490 res.mt.ptr[i+j*4] += mt.ptr[i+k*4]*m.mt.ptr[k+j*4];
2494 mt[] = res.mt[];
2495 return this;
2498 mat4 transposed () const {
2500 mat4 res;
2501 foreach (immutable i; 0..4) {
2502 foreach (immutable j; 0..4) {
2503 res.mt.ptr[i+j*4] = mt.ptr[j+i*4];
2506 return res;
2508 return mat4(
2509 mt.ptr[0], mt.ptr[4], mt.ptr[8], mt.ptr[12],
2510 mt.ptr[1], mt.ptr[5], mt.ptr[9], mt.ptr[13],
2511 mt.ptr[2], mt.ptr[6], mt.ptr[10], mt.ptr[14],
2512 mt.ptr[3], mt.ptr[7], mt.ptr[11], mt.ptr[15],
2516 void negate () { foreach (ref v; mt) v = -v; }
2518 mat4 opUnary(string op:"-") () const { pragma(inline, true); return this; }
2520 mat4 opUnary(string op:"-") () const {
2521 return mat4(
2522 -mt.ptr[0], -mt.ptr[1], -mt.ptr[2], -mt.ptr[3],
2523 -mt.ptr[4], -mt.ptr[5], -mt.ptr[6], -mt.ptr[7],
2524 -mt.ptr[8], -mt.ptr[9], -mt.ptr[10], -mt.ptr[11],
2525 -mt.ptr[12], -mt.ptr[13], -mt.ptr[14], -mt.ptr[15],
2529 // blends two matrices together, at a given percentage (range is [0..1]), blend==0: m2 is ignored
2530 // WARNING! won't sanitize `blend`
2531 mat4 blended() (in auto ref mat4 m2, Float blend) const {
2532 immutable Float ib = cast(Float)1-blend;
2533 mat4 res = void;
2534 res.mt.ptr[0] = mt.ptr[0]*ib+m2.mt.ptr[0]*blend;
2535 res.mt.ptr[1] = mt.ptr[1]*ib+m2.mt.ptr[1]*blend;
2536 res.mt.ptr[2] = mt.ptr[2]*ib+m2.mt.ptr[2]*blend;
2537 res.mt.ptr[3] = mt.ptr[3]*ib+m2.mt.ptr[3]*blend;
2538 res.mt.ptr[4] = mt.ptr[4]*ib+m2.mt.ptr[4]*blend;
2539 res.mt.ptr[5] = mt.ptr[5]*ib+m2.mt.ptr[5]*blend;
2540 res.mt.ptr[6] = mt.ptr[6]*ib+m2.mt.ptr[6]*blend;
2541 res.mt.ptr[7] = mt.ptr[7]*ib+m2.mt.ptr[7]*blend;
2542 res.mt.ptr[8] = mt.ptr[8]*ib+m2.mt.ptr[8]*blend;
2543 res.mt.ptr[9] = mt.ptr[9]*ib+m2.mt.ptr[9]*blend;
2544 res.mt.ptr[10] = mt.ptr[10]*ib+m2.mt.ptr[10]*blend;
2545 res.mt.ptr[11] = mt.ptr[11]*ib+m2.mt.ptr[11]*blend;
2546 res.mt.ptr[12] = mt.ptr[12]*ib+m2.mt.ptr[12]*blend;
2547 res.mt.ptr[13] = mt.ptr[13]*ib+m2.mt.ptr[13]*blend;
2548 res.mt.ptr[14] = mt.ptr[14]*ib+m2.mt.ptr[14]*blend;
2549 res.mt.ptr[15] = mt.ptr[15]*ib+m2.mt.ptr[15]*blend;
2550 return res;
2553 Float determinant() () const {
2554 return mt.ptr[0]*getCofactor(mt.ptr[5], mt.ptr[6], mt.ptr[7], mt.ptr[9], mt.ptr[10], mt.ptr[11], mt.ptr[13], mt.ptr[14], mt.ptr[15])-
2555 mt.ptr[1]*getCofactor(mt.ptr[4], mt.ptr[6], mt.ptr[7], mt.ptr[8], mt.ptr[10], mt.ptr[11], mt.ptr[12], mt.ptr[14], mt.ptr[15])+
2556 mt.ptr[2]*getCofactor(mt.ptr[4], mt.ptr[5], mt.ptr[7], mt.ptr[8], mt.ptr[9], mt.ptr[11], mt.ptr[12], mt.ptr[13], mt.ptr[15])-
2557 mt.ptr[3]*getCofactor(mt.ptr[4], mt.ptr[5], mt.ptr[6], mt.ptr[8], mt.ptr[9], mt.ptr[10], mt.ptr[12], mt.ptr[13], mt.ptr[14]);
2560 //WARNING: this must be tested for row/col
2561 // partially ;-) taken from DarkPlaces
2562 // this assumes uniform scaling
2563 mat4 invertedSimple () const {
2564 // we only support uniform scaling, so assume the first row is enough
2565 // (note the lack of sqrt here, because we're trying to undo the scaling,
2566 // this means multiplying by the inverse scale twice - squaring it, which
2567 // makes the sqrt a waste of time)
2568 version(all) {
2569 immutable Float scale = cast(Float)1/(mt.ptr[0*4+0]*mt.ptr[0*4+0]+mt.ptr[1*4+0]*mt.ptr[1*4+0]+mt.ptr[2*4+0]*mt.ptr[2*4+0]);
2570 } else {
2571 mixin(ImportCoreMath!(Float, "sqrt"));
2572 Float scale = cast(Float)3/sqrt(
2573 mt.ptr[0*4+0]*mt.ptr[0*4+0]+mt.ptr[1*4+0]*mt.ptr[1*4+0]+mt.ptr[2*4+0]*mt.ptr[2*4+0]+
2574 mt.ptr[0*4+1]*mt.ptr[0*4+1]+mt.ptr[1*4+1]*mt.ptr[1*4+1]+mt.ptr[2*4+1]*mt.ptr[2*4+1]+
2575 mt.ptr[0*4+2]*mt.ptr[0*4+2]+mt.ptr[1*4+2]*mt.ptr[1*4+2]+mt.ptr[2*4+2]*mt.ptr[2*4+2]
2577 scale *= scale;
2580 mat4 res = void;
2582 // invert the rotation by transposing and multiplying by the squared recipricol of the input matrix scale as described above
2583 res.mt.ptr[0*4+0] = mt.ptr[0*4+0]*scale;
2584 res.mt.ptr[1*4+0] = mt.ptr[0*4+1]*scale;
2585 res.mt.ptr[2*4+0] = mt.ptr[0*4+2]*scale;
2586 res.mt.ptr[0*4+1] = mt.ptr[1*4+0]*scale;
2587 res.mt.ptr[1*4+1] = mt.ptr[1*4+1]*scale;
2588 res.mt.ptr[2*4+1] = mt.ptr[1*4+2]*scale;
2589 res.mt.ptr[0*4+2] = mt.ptr[2*4+0]*scale;
2590 res.mt.ptr[1*4+2] = mt.ptr[2*4+1]*scale;
2591 res.mt.ptr[2*4+2] = mt.ptr[2*4+2]*scale;
2593 // invert the translate
2594 res.mt.ptr[3*4+0] = -(mt.ptr[3*4+0]*res.mt.ptr[0*4+0]+mt.ptr[3*4+1]*res.mt.ptr[1*4+0]+mt.ptr[3*4+2]*res.mt.ptr[2*4+0]);
2595 res.mt.ptr[3*4+1] = -(mt.ptr[3*4+0]*res.mt.ptr[0*4+1]+mt.ptr[3*4+1]*res.mt.ptr[1*4+1]+mt.ptr[3*4+2]*res.mt.ptr[2*4+1]);
2596 res.mt.ptr[3*4+2] = -(mt.ptr[3*4+0]*res.mt.ptr[0*4+2]+mt.ptr[3*4+1]*res.mt.ptr[1*4+2]+mt.ptr[3*4+2]*res.mt.ptr[2*4+2]);
2598 // don't know if there's anything worth doing here
2599 res.mt.ptr[0*4+3] = cast(Float)0;
2600 res.mt.ptr[1*4+3] = cast(Float)0;
2601 res.mt.ptr[2*4+3] = cast(Float)0;
2602 res.mt.ptr[3*4+3] = cast(Float)1;
2604 return res;
2607 //FIXME: make this fast pasta!
2608 ref mat4 invertSimple () {
2609 mt[] = invertedSimple().mt[];
2610 return this;
2613 // ////////////////////////////////////////////////////////////////////////////
2614 // compute the inverse of 4x4 Euclidean transformation matrix
2616 // Euclidean transformation is translation, rotation, and reflection.
2617 // With Euclidean transform, only the position and orientation of the object
2618 // will be changed. Euclidean transform does not change the shape of an object
2619 // (no scaling). Length and angle are reserved.
2621 // Use inverseAffine() if the matrix has scale and shear transformation.
2622 ref mat4 invertEuclidean() () {
2623 Float tmp = void;
2624 tmp = mt.ptr[1]; mt.ptr[1] = mt.ptr[4]; mt.ptr[4] = tmp;
2625 tmp = mt.ptr[2]; mt.ptr[2] = mt.ptr[8]; mt.ptr[8] = tmp;
2626 tmp = mt.ptr[6]; mt.ptr[6] = mt.ptr[9]; mt.ptr[9] = tmp;
2627 immutable Float x = mt.ptr[12];
2628 immutable Float y = mt.ptr[13];
2629 immutable Float z = mt.ptr[14];
2630 mt.ptr[12] = -(mt.ptr[0]*x+mt.ptr[4]*y+mt.ptr[8]*z);
2631 mt.ptr[13] = -(mt.ptr[1]*x+mt.ptr[5]*y+mt.ptr[9]*z);
2632 mt.ptr[14] = -(mt.ptr[2]*x+mt.ptr[6]*y+mt.ptr[10]*z);
2633 return this;
2636 // ////////////////////////////////////////////////////////////////////////////
2637 // compute the inverse of a 4x4 affine transformation matrix
2639 // Affine transformations are generalizations of Euclidean transformations.
2640 // Affine transformation includes translation, rotation, reflection, scaling,
2641 // and shearing. Length and angle are NOT preserved.
2642 ref mat4 invertAffine() () {
2643 // R^-1
2644 mixin(ImportCoreMath!(Float, "fabs"));
2645 // inverse 3x3 matrix
2646 Float[9] r = void; //[ mt.ptr[0],mt.ptr[1],mt.ptr[2], mt.ptr[4],mt.ptr[5],mt.ptr[6], mt.ptr[8],mt.ptr[9],mt.ptr[10] ];
2647 r.ptr[0] = mt.ptr[0];
2648 r.ptr[1] = mt.ptr[1];
2649 r.ptr[2] = mt.ptr[2];
2650 r.ptr[3] = mt.ptr[4];
2651 r.ptr[4] = mt.ptr[5];
2652 r.ptr[5] = mt.ptr[6];
2653 r.ptr[6] = mt.ptr[8];
2654 r.ptr[7] = mt.ptr[9];
2655 r.ptr[8] = mt.ptr[10];
2657 Float[9] tmp = void;
2658 tmp.ptr[0] = r.ptr[4]*r.ptr[8]-r.ptr[5]*r.ptr[7];
2659 tmp.ptr[1] = r.ptr[2]*r.ptr[7]-r.ptr[1]*r.ptr[8];
2660 tmp.ptr[2] = r.ptr[1]*r.ptr[5]-r.ptr[2]*r.ptr[4];
2661 tmp.ptr[3] = r.ptr[5]*r.ptr[6]-r.ptr[3]*r.ptr[8];
2662 tmp.ptr[4] = r.ptr[0]*r.ptr[8]-r.ptr[2]*r.ptr[6];
2663 tmp.ptr[5] = r.ptr[2]*r.ptr[3]-r.ptr[0]*r.ptr[5];
2664 tmp.ptr[6] = r.ptr[3]*r.ptr[7]-r.ptr[4]*r.ptr[6];
2665 tmp.ptr[7] = r.ptr[1]*r.ptr[6]-r.ptr[0]*r.ptr[7];
2666 tmp.ptr[8] = r.ptr[0]*r.ptr[4]-r.ptr[1]*r.ptr[3];
2667 // check determinant if it is 0
2668 immutable Float determinant = r.ptr[0]*tmp.ptr[0]+r.ptr[1]*tmp.ptr[3]+r.ptr[2]*tmp.ptr[6];
2669 if (fabs(determinant) <= EPSILON!Float) {
2670 // cannot inverse, make it idenety matrix
2671 r[] = 0;
2672 r.ptr[0] = r.ptr[4] = r.ptr[8] = 1;
2673 } else {
2674 // divide by the determinant
2675 immutable Float invDeterminant = cast(Float)1/determinant;
2676 r.ptr[0] = invDeterminant*tmp.ptr[0];
2677 r.ptr[1] = invDeterminant*tmp.ptr[1];
2678 r.ptr[2] = invDeterminant*tmp.ptr[2];
2679 r.ptr[3] = invDeterminant*tmp.ptr[3];
2680 r.ptr[4] = invDeterminant*tmp.ptr[4];
2681 r.ptr[5] = invDeterminant*tmp.ptr[5];
2682 r.ptr[6] = invDeterminant*tmp.ptr[6];
2683 r.ptr[7] = invDeterminant*tmp.ptr[7];
2684 r.ptr[8] = invDeterminant*tmp.ptr[8];
2688 mt.ptr[0] = r.ptr[0]; mt.ptr[1] = r.ptr[1]; mt.ptr[2] = r.ptr[2];
2689 mt.ptr[4] = r.ptr[3]; mt.ptr[5] = r.ptr[4]; mt.ptr[6] = r.ptr[5];
2690 mt.ptr[8] = r.ptr[6]; mt.ptr[9] = r.ptr[7]; mt.ptr[10]= r.ptr[8];
2692 // -R^-1 * T
2693 immutable Float x = mt.ptr[12];
2694 immutable Float y = mt.ptr[13];
2695 immutable Float z = mt.ptr[14];
2696 mt.ptr[12] = -(r.ptr[0]*x+r.ptr[3]*y+r.ptr[6]*z);
2697 mt.ptr[13] = -(r.ptr[1]*x+r.ptr[4]*y+r.ptr[7]*z);
2698 mt.ptr[14] = -(r.ptr[2]*x+r.ptr[5]*y+r.ptr[8]*z);
2700 // last row should be unchanged (0,0,0,1)
2701 //mt.ptr[3] = mt.ptr[7] = mt.ptr[11] = 0.0f;
2702 //mt.ptr[15] = 1.0f;
2704 return this;
2707 ref mat4 invert() () {
2708 // if the 4th row is [0,0,0,1] then it is affine matrix and
2709 // it has no projective transformation
2710 if (mt.ptr[3] == 0 && mt.ptr[7] == 0 && mt.ptr[11] == 0 && mt.ptr[15] == 1) {
2711 return invertedAffine();
2712 } else {
2713 return invertedGeneral();
2717 ///////////////////////////////////////////////////////////////////////////////
2718 // compute the inverse of a general 4x4 matrix using Cramer's Rule
2719 // if cannot find inverse, return indentity matrix
2720 ref mat4 invertGeneral() () {
2721 mixin(ImportCoreMath!(Float, "fabs"));
2723 immutable Float cofactor0 = getCofactor(mt.ptr[5], mt.ptr[6], mt.ptr[7], mt.ptr[9], mt.ptr[10], mt.ptr[11], mt.ptr[13], mt.ptr[14], mt.ptr[15]);
2724 immutable Float cofactor1 = getCofactor(mt.ptr[4], mt.ptr[6], mt.ptr[7], mt.ptr[8], mt.ptr[10], mt.ptr[11], mt.ptr[12], mt.ptr[14], mt.ptr[15]);
2725 immutable Float cofactor2 = getCofactor(mt.ptr[4], mt.ptr[5], mt.ptr[7], mt.ptr[8], mt.ptr[9], mt.ptr[11], mt.ptr[12], mt.ptr[13], mt.ptr[15]);
2726 immutable Float cofactor3 = getCofactor(mt.ptr[4], mt.ptr[5], mt.ptr[6], mt.ptr[8], mt.ptr[9], mt.ptr[10], mt.ptr[12], mt.ptr[13], mt.ptr[14]);
2728 immutable Float determinant = mt.ptr[0]*cofactor0-mt.ptr[1]*cofactor1+mt.ptr[2]*cofactor2-mt.ptr[3]*cofactor3;
2729 if (fabs(determinant) <= EPSILON!Float) { this = Identity; return this; }
2731 immutable Float cofactor4 = getCofactor(mt.ptr[1], mt.ptr[2], mt.ptr[3], mt.ptr[9], mt.ptr[10], mt.ptr[11], mt.ptr[13], mt.ptr[14], mt.ptr[15]);
2732 immutable Float cofactor5 = getCofactor(mt.ptr[0], mt.ptr[2], mt.ptr[3], mt.ptr[8], mt.ptr[10], mt.ptr[11], mt.ptr[12], mt.ptr[14], mt.ptr[15]);
2733 immutable Float cofactor6 = getCofactor(mt.ptr[0], mt.ptr[1], mt.ptr[3], mt.ptr[8], mt.ptr[9], mt.ptr[11], mt.ptr[12], mt.ptr[13], mt.ptr[15]);
2734 immutable Float cofactor7 = getCofactor(mt.ptr[0], mt.ptr[1], mt.ptr[2], mt.ptr[8], mt.ptr[9], mt.ptr[10], mt.ptr[12], mt.ptr[13], mt.ptr[14]);
2736 immutable Float cofactor8 = getCofactor(mt.ptr[1], mt.ptr[2], mt.ptr[3], mt.ptr[5], mt.ptr[6], mt.ptr[7], mt.ptr[13], mt.ptr[14], mt.ptr[15]);
2737 immutable Float cofactor9 = getCofactor(mt.ptr[0], mt.ptr[2], mt.ptr[3], mt.ptr[4], mt.ptr[6], mt.ptr[7], mt.ptr[12], mt.ptr[14], mt.ptr[15]);
2738 immutable Float cofactor10= getCofactor(mt.ptr[0], mt.ptr[1], mt.ptr[3], mt.ptr[4], mt.ptr[5], mt.ptr[7], mt.ptr[12], mt.ptr[13], mt.ptr[15]);
2739 immutable Float cofactor11= getCofactor(mt.ptr[0], mt.ptr[1], mt.ptr[2], mt.ptr[4], mt.ptr[5], mt.ptr[6], mt.ptr[12], mt.ptr[13], mt.ptr[14]);
2741 immutable Float cofactor12= getCofactor(mt.ptr[1], mt.ptr[2], mt.ptr[3], mt.ptr[5], mt.ptr[6], mt.ptr[7], mt.ptr[9], mt.ptr[10], mt.ptr[11]);
2742 immutable Float cofactor13= getCofactor(mt.ptr[0], mt.ptr[2], mt.ptr[3], mt.ptr[4], mt.ptr[6], mt.ptr[7], mt.ptr[8], mt.ptr[10], mt.ptr[11]);
2743 immutable Float cofactor14= getCofactor(mt.ptr[0], mt.ptr[1], mt.ptr[3], mt.ptr[4], mt.ptr[5], mt.ptr[7], mt.ptr[8], mt.ptr[9], mt.ptr[11]);
2744 immutable Float cofactor15= getCofactor(mt.ptr[0], mt.ptr[1], mt.ptr[2], mt.ptr[4], mt.ptr[5], mt.ptr[6], mt.ptr[8], mt.ptr[9], mt.ptr[10]);
2746 immutable Float invDeterminant = cast(Float)1/determinant;
2747 mt.ptr[0] = invDeterminant*cofactor0;
2748 mt.ptr[1] = -invDeterminant*cofactor4;
2749 mt.ptr[2] = invDeterminant*cofactor8;
2750 mt.ptr[3] = -invDeterminant*cofactor12;
2752 mt.ptr[4] = -invDeterminant*cofactor1;
2753 mt.ptr[5] = invDeterminant*cofactor5;
2754 mt.ptr[6] = -invDeterminant*cofactor9;
2755 mt.ptr[7] = invDeterminant*cofactor13;
2757 mt.ptr[8] = invDeterminant*cofactor2;
2758 mt.ptr[9] = -invDeterminant*cofactor6;
2759 mt.ptr[10]= invDeterminant*cofactor10;
2760 mt.ptr[11]= -invDeterminant*cofactor14;
2762 mt.ptr[12]= -invDeterminant*cofactor3;
2763 mt.ptr[13]= invDeterminant*cofactor7;
2764 mt.ptr[14]= -invDeterminant*cofactor11;
2765 mt.ptr[15]= invDeterminant*cofactor15;
2767 return this;
2770 // compute cofactor of 3x3 minor matrix without sign
2771 // input params are 9 elements of the minor matrix
2772 // NOTE: The caller must know its sign
2773 private static Float getCofactor() (Float m0, Float m1, Float m2, Float m3, Float m4, Float m5, Float m6, Float m7, Float m8) {
2774 pragma(inline, true);
2775 return m0*(m4*m8-m5*m7)-m1*(m3*m8-m5*m6)+m2*(m3*m7-m4*m6);
2778 Quat4!VT toQuaternion () const nothrow @trusted @nogc {
2779 mixin(ImportCoreMath!(Float, "sqrt"));
2780 Quat4!VT res = void;
2781 immutable Float tr = mt.ptr[0*4+0]+mt.ptr[1*4+1]+mt.ptr[2*4+2];
2782 // check the diagonal
2783 if (tr > 0) {
2784 Float s = sqrt(tr+cast(Float)1);
2785 res.w = s/cast(Float)2;
2786 s = cast(Float)0.5/s;
2787 res.x = (mt.ptr[2*4+1]-mt.ptr[1*4+2])*s;
2788 res.y = (mt.ptr[0*4+2]-mt.ptr[2*4+0])*s;
2789 res.z = (mt.ptr[1*4+0]-mt.ptr[0*4+1])*s;
2790 } else {
2791 // diagonal is negative
2792 int[3] nxt = [1, 2, 0];
2793 int i = 0;
2794 if (mt.ptr[1*4+1] > mt.ptr[0*4+0]) i = 1;
2795 if (mt.ptr[2*4+2] > mt.ptr[i*4+i]) i = 2;
2796 int j = nxt.ptr[i];
2797 int k = nxt.ptr[j];
2798 Float s = sqrt((mt.ptr[i*4+i]-(mt.ptr[j*4+j]+mt.ptr[k*4+k]))+cast(Float)1);
2799 Float[4] q = void;
2800 q.ptr[i] = s*cast(Float)0.5;
2801 if (s != 0) s = cast(Float)0.5/s;
2802 q.ptr[3] = (mt.ptr[k*4+j]-mt.ptr[j*4+k])*s;
2803 q.ptr[j] = (mt.ptr[j*4+i]+mt.ptr[i*4+j])*s;
2804 q.ptr[k] = (mt.ptr[k*4+i]+mt.ptr[i*4+k])*s;
2805 res.x = q.ptr[0];
2806 res.y = q.ptr[1];
2807 res.z = q.ptr[2];
2808 res.w = q.ptr[3];
2810 return res;
2815 // ////////////////////////////////////////////////////////////////////////// //
2816 alias quat4 = Quat4!vec3;
2818 align(1) struct Quat4(VT) if (IsVector!VT) {
2819 align(1):
2820 public:
2821 alias Float = VT.Float;
2822 alias quat4 = typeof(this);
2824 public:
2825 Float w=1, x=0, y=0, z=0; // default: identity
2827 public:
2828 this (Float aw, Float ax, Float ay, Float az) nothrow @trusted @nogc {
2829 pragma(inline, true);
2830 w = aw;
2831 x = ax;
2832 y = ay;
2833 z = az;
2836 // valid only for unit quaternions
2837 this (Float ax, Float ay, Float az) nothrow @trusted @nogc {
2838 immutable Float t = cast(Float)1-(ax*ax)-(ay*ay)-(az*az);
2839 if (t < 0) {
2840 w = 0;
2841 } else {
2842 mixin(ImportCoreMath!(Float, "sqrt"));
2843 w = -sqrt(t);
2845 x = ax;
2846 y = ay;
2847 z = az;
2850 this() (in auto ref VT v) nothrow @safe @nogc {
2851 pragma(inline, true);
2852 x = v.x;
2853 y = v.y;
2854 z = v.z;
2855 w = 0;
2858 // the rotation of a point by a quaternion is given by the formula:
2859 // R = Q.P.Q*
2860 // where R is the resultant quaternion, Q is the orientation quaternion by which you want to perform a rotation,
2861 // Q* the conjugate of Q and P is the point converted to a quaternion.
2862 // note: here the "." is the multiplication operator.
2864 // to convert a 3D vector to a quaternion, copy the x, y and z components and set the w component to 0.
2865 // this is the same for quaternion to vector conversion: take the x, y and z components and forget the w.
2867 VT asVector () const nothrow @safe @nogc { pragma(inline, true); return VT(x, y, z); }
2869 static quat4 Identity () nothrow @safe @nogc { pragma(inline, true); return quat4(1, 0, 0, 0); }
2870 bool isIdentity () const nothrow @safe @nogc { pragma(inline, true); return (w == 1 && x == 0 && y == 0 && z == 0); }
2872 static quat4 fromAngles (Float roll, Float pitch, Float yaw) nothrow @trusted @nogc {
2873 mixin(ImportCoreMath!(Float, "cos", "sin"));
2874 // calculate trig identities
2875 immutable Float cr = cos(roll/2);
2876 immutable Float cp = cos(pitch/2);
2877 immutable Float cy = cos(yaw/2);
2878 immutable Float sr = sin(roll/2);
2879 immutable Float sp = sin(pitch/2);
2880 immutable Float sy = sin(yaw/2);
2881 immutable Float cpcy = cp*cy;
2882 immutable Float spsy = sp*sy;
2883 return quat4(
2884 cr*cpcy+sr*spsy,
2885 sr*cpcy-cr*spsy,
2886 cr*sp*cy+sr*cp*sy,
2887 cr*cp*sy-sr*sp*cy,
2891 //@property bool valid () const nothrow @safe @nogc { pragma(inline, true); import core.stdc.math : isnan; return !isnan(w) && !isnan(x) && !isnan(y) && !isnan(z); }
2892 @property bool valid () const nothrow @safe @nogc { pragma(inline, true); import core.stdc.math : isfinite; return isfinite(w) && isfinite(x) && isfinite(y) && isfinite(z); }
2894 // x,y,z,w
2895 Float opIndex (usize idx) const nothrow @safe @nogc {
2896 pragma(inline, true);
2897 return
2898 idx == 0 ? x :
2899 idx == 1 ? y :
2900 idx == 2 ? z :
2901 idx == 3 ? w :
2902 Float.nan;
2905 // x,y,z,w
2906 void opIndexAssign (Float v, usize idx) {
2907 pragma(inline, true);
2908 if (idx == 0) x = v; else
2909 if (idx == 1) y = v; else
2910 if (idx == 2) z = v; else
2911 if (idx == 3) w = v; else
2912 assert(0, "invalid quaternion index");
2915 Mat4!VT toMatrix () const nothrow @trusted @nogc {
2916 // calculate coefficients
2917 immutable Float x2 = this.x+this.x;
2918 immutable Float y2 = this.y+this.y;
2919 immutable Float z2 = this.z+this.z;
2920 immutable Float xx = this.x*x2;
2921 immutable Float xy = this.x*y2;
2922 immutable Float xz = this.x*z2;
2923 immutable Float yy = this.y*y2;
2924 immutable Float yz = this.y*z2;
2925 immutable Float zz = this.z*z2;
2926 immutable Float wx = this.w*x2;
2927 immutable Float wy = this.w*y2;
2928 immutable Float wz = this.w*z2;
2930 Mat4!VT res = void;
2931 res.mt.ptr[0*4+0] = cast(Float)1-(yy+zz);
2932 res.mt.ptr[0*4+1] = xy-wz;
2933 res.mt.ptr[0*4+2] = xz+wy;
2934 res.mt.ptr[0*4+3] = 0;
2936 res.mt.ptr[1*4+0] = xy+wz;
2937 res.mt.ptr[1*4+1] = cast(Float)1-(xx+zz);
2938 res.mt.ptr[1*4+2] = yz-wx;
2939 res.mt.ptr[1*4+3] = 0;
2941 res.mt.ptr[2*4+0] = xz-wy;
2942 res.mt.ptr[2*4+1] = yz+wx;
2943 res.mt.ptr[2*4+2] = cast(Float)1-(xx+yy);
2944 res.mt.ptr[2*4+3] = 0;
2946 res.mt.ptr[3*4+0] = 0;
2947 res.mt.ptr[3*4+1] = 0;
2948 res.mt.ptr[3*4+2] = 0;
2949 res.mt.ptr[3*4+3] = 1;
2951 return res;
2954 auto opUnary(string op:"+") () const nothrow @safe @nogc { pragma(inline, true); return this; }
2956 // for unit quaternions, this is inverse/conjugate
2957 auto opUnary(string op:"-") () const nothrow @safe @nogc { pragma(inline, true); return quat4(-w, -x, -y, -z); }
2959 quat4 opBinary(string op:"*") (in auto ref quat4 q2) const nothrow @safe @nogc {
2960 auto res = quat4(this.w, this.x, this.y, this.z);
2961 return (res *= q2);
2964 ref quat4 opOpAssign(string op:"*") (in auto ref quat4 q2) nothrow @safe @nogc {
2965 immutable Float A = (this.w+this.x)*(q2.w+q2.x);
2966 immutable Float B = (this.z-this.y)*(q2.y-q2.z);
2967 immutable Float C = (this.w-this.x)*(q2.y+q2.z);
2968 immutable Float D = (this.y+this.z)*(q2.w-q2.x);
2969 immutable Float E = (this.x+this.z)*(q2.x+q2.y);
2970 immutable Float F = (this.x-this.z)*(q2.x-q2.y);
2971 immutable Float G = (this.w+this.y)*(q2.w-q2.z);
2972 immutable Float H = (this.w-this.y)*(q2.w+q2.z);
2973 this.w = B+(-E-F+G+H)/2;
2974 this.x = A-(E+F+G+H)/2;
2975 this.y = C+(E-F+G-H)/2;
2976 this.z = D+(E-F-G+H)/2;
2977 return this;
2980 quat4 slerp() (in auto ref quat4 to, Float t) const nothrow @trusted @nogc {
2981 mixin(ImportCoreMath!(Float, "acos", "sin"));
2982 Float[4] to1 = void;
2983 // calc cosine
2984 Float cosom = this.x*to.x+this.y*to.y+this.z*to.z+this.w*to.w;
2985 // adjust signs (if necessary)
2986 if (cosom < 0) {
2987 cosom = -cosom;
2988 to1.ptr[0] = -to.x;
2989 to1.ptr[1] = -to.y;
2990 to1.ptr[2] = -to.z;
2991 to1.ptr[3] = -to.w;
2992 } else {
2993 to1.ptr[0] = to.x;
2994 to1.ptr[1] = to.y;
2995 to1.ptr[2] = to.z;
2996 to1.ptr[3] = to.w;
2998 Float scale0 = void, scale1 = void;
2999 // calculate coefficients
3000 if (cast(Float)1-cosom > EPSILON!Float) {
3001 // standard case (slerp)
3002 immutable Float omega = acos(cosom);
3003 immutable Float sinom = sin(omega);
3004 scale0 = sin((cast(Float)1-t)*omega)/sinom;
3005 scale1 = sin(t*omega)/sinom;
3006 } else {
3007 // "from" and "to" quaternions are very close, so we can do a linear interpolation
3008 scale0 = cast(Float)1-t;
3009 scale1 = t;
3011 // calculate final values
3012 return quat4(
3013 scale0*this.w+scale1*to1.ptr[3],
3014 scale0*this.x+scale1*to1.ptr[0],
3015 scale0*this.y+scale1*to1.ptr[1],
3016 scale0*this.z+scale1*to1.ptr[2],
3022 // ////////////////////////////////////////////////////////////////////////// //
3023 alias OBB2D = OBB2d!vec2;
3025 /// Oriented Bounding Box
3026 struct OBB2d(VT) if (IsVectorDim!(VT, 2)) {
3027 // to make the tests extremely efficient, `origin` stores the
3028 // projection of corner number zero onto a box's axes and the axes are stored
3029 // explicitly in axis. the magnitude of these stored axes is the inverse
3030 // of the corresponding edge length so that all overlap tests can be performed on
3031 // the interval [0, 1] without normalization, and square roots are avoided
3032 // throughout the entire test.
3033 public:
3034 alias Me = typeof(this);
3035 alias Float = VT.Float;
3036 alias vec2 = VT;
3038 private:
3039 VT[4] corner; // corners of the box, where 0 is the lower left
3040 bool aovalid; // are axes and origin valid?
3041 VT[2] axis; // two edges of the box extended away from corner[0]
3042 VT.Float[2] origin; // origin[a] = corner[0].dot(axis[a]);
3044 private nothrow @trusted @nogc:
3045 // returns true if other overlaps one dimension of this
3046 bool overlaps1Way() (in auto ref Me other) const {
3047 foreach (immutable a; 0..2) {
3048 Float t = other.corner.ptr[0].dot(axis.ptr[a]);
3049 // find the extent of box 2 on axis a
3050 Float tMin = t, tMax = t;
3051 foreach (immutable c; 1..4) {
3052 t = other.corner.ptr[c].dot(axis.ptr[a]);
3053 if (t < tMin) tMin = t; else if (t > tMax) tMax = t;
3055 // we have to subtract off the origin
3056 // see if [tMin, tMax] intersects [0, 1]
3057 if (tMin > 1+origin.ptr[a] || tMax < origin.ptr[a]) {
3058 // there was no intersection along this dimension; the boxes cannot possibly overlap
3059 return false;
3062 // there was no dimension along which there is no intersection: therefore the boxes overlap
3063 return true;
3066 // updates the axes after the corners move; assumes the corners actually form a rectangle
3067 void computeAxes () {
3068 axis.ptr[0] = corner.ptr[1]-corner.ptr[0];
3069 axis.ptr[1] = corner.ptr[3]-corner.ptr[0];
3070 // make the length of each axis 1/edge length so we know any dot product must be less than 1 to fall within the edge
3071 foreach (immutable a; 0..2) {
3072 axis.ptr[a] /= axis.ptr[a].lengthSquared;
3073 origin.ptr[a] = corner.ptr[0].dot(axis.ptr[a]);
3075 aovalid = true;
3078 public:
3080 this() (in auto ref VT center, in Float w, in Float h, in Float angle) {
3081 mixin(ImportCoreMath!(Float, "cos", "sin"));
3083 immutable Float ca = cos(angle);
3084 immutable Float sa = sin(angle);
3085 auto ox = VT( ca, sa);
3086 auto oy = VT(-sa, ca);
3088 ox *= w/2;
3089 oy *= h/2;
3091 corner.ptr[0] = center-ox-oy;
3092 corner.ptr[1] = center+ox-oy;
3093 corner.ptr[2] = center+ox+oy;
3094 corner.ptr[3] = center-ox+oy;
3096 // compute axes on demand
3097 //computeAxes();
3100 VT[4] corners () const { pragma(inline, true); VT[4] res = corner[]; return res; }
3102 /// get corner
3103 VT opIndex (usize idx) const {
3104 pragma(inline, true);
3105 return (idx < 4 ? corner.ptr[idx] : VT(Float.nan, Float.nan, Float.nan));
3109 void moveTo() (in auto ref VT center) {
3110 immutable centroid = (corner.ptr[0]+corner.ptr[1]+corner.ptr[2]+corner.ptr[3])/4;
3111 immutable translation = center-centroid;
3112 foreach (ref VT cv; corner[]) cv += translation;
3113 aovalid = false; // invalidate axes
3117 void moveBy() (in auto ref VT delta) {
3118 foreach (ref VT cv; corner[]) cv += delta;
3119 aovalid = false; // invalidate axes
3122 /// rotate around centroid
3123 void rotate() (Float angle) {
3124 mixin(ImportCoreMath!(Float, "cos", "sin"));
3125 immutable centroid = (corner.ptr[0]+corner.ptr[1]+corner.ptr[2]+corner.ptr[3])/4;
3126 immutable Float ca = cos(angle);
3127 immutable Float sa = sin(angle);
3128 foreach (ref cv; corner[]) {
3129 immutable Float ox = cv.x-centroid.x;
3130 immutable Float oy = cv.y-centroid.y;
3131 cv.x = ox*ca-oy*sa+centroid.x;
3132 cv.y = ox*sa+oy*ca+centroid.y;
3134 aovalid = false; // invalidate axes
3138 void scale() (Float mult) {
3139 immutable centroid = (corner.ptr[0]+corner.ptr[1]+corner.ptr[2]+corner.ptr[3])/4;
3140 foreach (ref cv; corner[]) cv = (cv-centroid)*mult+centroid;
3141 aovalid = false; // invalidate axes
3144 /// apply transformation matrix, assuming that world (0,0) is at centroid
3145 void transform() (in auto ref Mat3!VT mat) {
3146 immutable centroid = (corner.ptr[0]+corner.ptr[1]+corner.ptr[2]+corner.ptr[3])/4;
3147 foreach (ref VT cv; corner[]) cv -= centroid;
3148 foreach (ref VT cv; corner[]) cv = cv*mat;
3149 foreach (ref VT cv; corner[]) cv += centroid;
3150 aovalid = false; // invalidate axes
3153 /// returns true if the intersection of the boxes is non-empty
3154 bool overlaps() (in auto ref Me other) {
3155 pragma(inline, true);
3156 if (!aovalid) computeAxes(); // fix axes if necessary
3157 return (overlaps1Way(other) && other.overlaps1Way(this));
3162 // ////////////////////////////////////////////////////////////////////////// //
3163 template IsSphere(ST) {
3164 static if (is(ST == Sphere!SVT, SVT)) {
3165 enum IsSphere = IsVector!SVT;
3166 } else {
3167 enum IsSphere = false;
3171 template IsSphere(ST, VT) {
3172 static if (is(ST == Sphere!SVT, SVT)) {
3173 enum IsSphere = (is(VT == SVT) && IsVector!SVT);
3174 } else {
3175 enum IsSphere = false;
3181 struct Sphere(VT) if (IsVector!VT) {
3182 public:
3183 alias Float = VT.Float;
3184 alias vec = VT;
3185 alias Me = typeof(this);
3187 public:
3188 vec orig; /// sphere origin
3189 Float radius; /// sphere radius
3191 public nothrow @trusted @nogc:
3192 this() (in auto ref vec aorig, Float arad) { orig = aorig; radius = arad; }
3194 @property bool valid () const { import core.stdc.math : isfinite; pragma(inline, true); return (isfinite(radius) && radius > 0); }
3196 /// sweep test
3197 bool sweep() (in auto ref vec amove, in auto ref Me sb, Float* u0, Float* u1) const {
3198 mixin(ImportCoreMath!(Float, "sqrt"));
3200 immutable odist = sb.orig-this.orig; // vector from A0 to B0
3201 immutable vab = -amove; // relative velocity (in normalized time)
3202 immutable rab = this.radius+sb.radius;
3203 immutable Float a = vab.dot(vab); // u*u coefficient
3204 immutable Float b = cast(Float)2*vab.dot(odist); // u coefficient
3205 immutable Float c = odist.dot(odist)-rab*rab; // constant term
3207 // check if they're currently overlapping
3208 if (odist.dot(odist) <= rab*rab) {
3209 if (u0 !is null) *u0 = 0;
3210 if (u0 !is null) *u1 = 0;
3211 return true;
3214 // check if they hit each other during the frame
3215 immutable Float q = b*b-4*a*c;
3216 if (q < 0) return false; // alas, complex roots
3218 immutable Float sq = sqrt(q);
3219 immutable Float d = cast(Float)1/(cast(Float)2*a);
3220 Float uu0 = (-b+sq)*d;
3221 Float uu1 = (-b-sq)*d;
3223 if (uu0 > uu1) { immutable t = uu0; uu0 = uu1; uu1 = t; } // swap
3224 if (u0 !is null) *u0 = uu0;
3225 if (u1 !is null) *u1 = uu1;
3227 return true;
3230 // sweep test; if `true` (hit), `hitpos` will be sphere position when it hits this plane, and `u` will be normalized collision time
3231 bool sweep(PT) (in auto ref PT plane, in auto ref vec3 amove, vec3* hitpos, Float* u) const if (IsPlane3!(PT, Float)) {
3232 pragma(inline, true);
3233 return plane.sweep(this, amove, hitpos, u);
3238 // ////////////////////////////////////////////////////////////////////////// //
3239 template IsPlane3(PT) {
3240 static if (is(PT == Plane3!(PFT, eps, swiz), PFT, double eps, bool swiz)) {
3241 enum IsPlane3 = (is(PFT == float) || is(PFT == double));
3242 } else {
3243 enum IsPlane3 = false;
3248 template IsPlane3(PT, FT) {
3249 static if (is(PT == Plane3!(PFT, eps, swiz), PFT, double eps, bool swiz)) {
3250 enum IsPlane3 = (is(FT == PFT) && (is(PFT == float) || is(PFT == double)));
3251 } else {
3252 enum IsPlane3 = false;
3257 // plane in 3D space: Ax+By+Cz+D=0
3258 align(1) struct Plane3(FloatType=VFloat, double PlaneEps=-1.0, bool enableSwizzling=true) if (is(FloatType == float) || is(FloatType == double)) {
3259 align(1):
3260 public:
3261 alias Float = FloatType;
3262 alias plane3 = typeof(this);
3263 alias Me = typeof(this);
3264 alias vec3 = VecN!(3, Float);
3265 static if (PlaneEps < 0) {
3266 enum EPS = EPSILON!Float;
3267 } else {
3268 enum EPS = cast(Float)PlaneEps;
3271 public:
3272 alias PType = ubyte;
3273 enum /*PType*/ {
3274 Coplanar = 0,
3275 Front = 1,
3276 Back = 2,
3277 Spanning = 3,
3280 public:
3281 //Float a = 0, b = 0, c = 0, d = 0;
3282 vec3 normal;
3283 Float w;
3285 nothrow @safe:
3286 string toString () const {
3287 import std.string : format;
3288 try {
3289 return "(%s,%s,%s,%s)".format(normal.x, normal.y, normal.z, w);
3290 } catch (Exception) {
3291 assert(0);
3295 @nogc:
3296 this() (in auto ref vec3 aorigin, in auto ref vec3 anormal) { pragma(inline, true); setOriginNormal(aorigin, anormal); }
3297 this() (in auto ref vec3 anormal, Float aw) { pragma(inline, true); set(anormal, aw); }
3298 this() (in auto ref vec3 a, in auto ref vec3 b, in auto ref vec3 c) { pragma(inline, true); setFromPoints(a, b, c); }
3300 @property Float offset () const pure { pragma(inline, true); return -w; }
3302 void set () (in auto ref vec3 anormal, Float aw) {
3303 pragma(inline, true);
3304 normal = anormal;
3305 w = aw;
3308 void setOriginNormal () (in auto ref vec3 aorigin, in auto ref vec3 anormal) {
3309 normal = anormal.normalized;
3310 //origin = aorigin;
3311 w = normal.x*aorigin.x+normal.y*aorigin.y+normal.z*aorigin.z;
3314 void setFromPoints() (in auto ref vec3 a, in auto ref vec3 b, in auto ref vec3 c) @trusted {
3315 //normal = ((b-a)^(c-a)).normalized;
3316 immutable vec3 b0 = b-a;
3317 immutable vec3 c0 = c-a;
3318 normal = vec3(b0.y*c0.z-b0.z*c0.y, b0.z*c0.x-b0.x*c0.z, b0.x*c0.y-b0.y*c0.x);
3319 version(none) {
3320 normal.normalize;
3321 //x*a.x+y*a.y+z*a.z
3322 //w = normal*a; // n.dot(a)
3323 w = normal.x*a.x+normal.y*a.y+normal.z*a.z;
3324 //immutable ow = w;
3325 //normalize; // just in case, to tolerate some floating point errors
3326 //assert(ow == w);
3327 } else {
3328 immutable double len = normal.dbllength;
3329 //{ import core.stdc.stdio; printf("**len=%g\n", len); }
3330 if (len <= EPSILON!Float) {
3331 // oops
3332 //{ import core.stdc.stdio; printf(" OOPS: n=(%g,%g,%g)\n", cast(double)normal.x, cast(double)normal.y, cast(double)normal.z); }
3333 normal = vec3.Invalid;
3334 w = vec3.Float.nan;
3335 return;
3337 version(vmath_slow_normalize) {
3338 normal.x /= len;
3339 normal.y /= len;
3340 normal.z /= len;
3341 } else {
3342 immutable double dd = 1.0/len;
3343 normal.x *= dd;
3344 normal.y *= dd;
3345 normal.z *= dd;
3347 w = normal.x*a.x+normal.y*a.y+normal.z*a.z;
3349 immutable ow = w;
3350 normalize; // just in case, to tolerate some floating point errors
3351 assert(ow == w);
3354 assert(valid);
3355 //return this;
3358 @property bool valid () const { pragma(inline, true); import core.stdc.math : isfinite; return (isfinite(w) != 0); }
3360 Float opIndex (usize idx) const {
3361 pragma(inline, true);
3362 return (idx == 0 ? normal.x : idx == 1 ? normal.y : idx == 2 ? normal.z : idx == 3 ? w : Float.nan);
3365 void opIndexAssign (Float v, usize idx) {
3366 pragma(inline, true);
3367 if (idx == 0) normal.x = v; else if (idx == 1) normal.y = v; else if (idx == 2) normal.z = v; else if (idx == 3) w = v;
3370 ref plane3 normalize () {
3371 if (!normal.isFinite) {
3372 normal = vec3.Invalid;
3373 w = vec3.Float.nan;
3374 return this;
3376 version(none) {
3377 mixin(ImportCoreMath!(Float, "fabs"));
3378 double dd = normal.dbllength;
3379 if (fabs(1.0-dd) > EPSILON!Float) {
3380 version(vmath_slow_normalize) {
3381 normal.x /= dd;
3382 normal.y /= dd;
3383 normal.z /= dd;
3384 w /= dd;
3385 } else {
3386 dd = cast(Float)1/dd;
3387 normal.x *= dd;
3388 normal.y *= dd;
3389 normal.z *= dd;
3390 w *= dd;
3393 } else {
3394 mixin(ImportCoreMath!(double, "sqrt"));
3395 double dd = sqrt(cast(double)normal.x*cast(double)normal.x+cast(double)normal.y*cast(double)normal.y+cast(double)normal.z*cast(double)normal.z);
3396 version(vmath_slow_normalize) {
3397 normal.x /= dd;
3398 normal.y /= dd;
3399 normal.z /= dd;
3400 w /= dd;
3401 } else {
3402 dd = 1.0/dd;
3403 normal.x *= dd;
3404 normal.y *= dd;
3405 normal.z *= dd;
3406 w *= dd;
3409 return this;
3412 //WARNING! won't check if this plane is valid
3413 void flip () {
3414 pragma(inline, true);
3415 normal = -normal;
3416 w = -w;
3419 //WARNING! won't check if this plane is valid
3420 plane3 fliped () const {
3421 pragma(inline, true);
3422 return plane3(-normal, -w);
3425 PType pointSide() (in auto ref vec3 p) const {
3426 pragma(inline, true);
3427 //immutable Float t = (normal*p)-w; // dot
3428 immutable double t = cast(double)normal.x*cast(double)p.x+cast(double)normal.y*cast(double)p.y+cast(double)normal.z*cast(double)p.z-cast(double)w;
3429 return (t < -EPS ? Back : (t > EPS ? Front : Coplanar));
3432 double pointSideD() (in auto ref vec3 p) const {
3433 pragma(inline, true);
3434 //return (normal*p)-w; // dot
3435 return cast(double)normal.x*cast(double)p.x+cast(double)normal.y*cast(double)p.y+cast(double)normal.z*cast(double)p.z-cast(double)w;
3438 Float pointSideF() (in auto ref vec3 p) const {
3439 pragma(inline, true);
3440 //return (normal*p)-w; // dot
3441 return cast(Float)(cast(double)normal.x*cast(double)p.x+cast(double)normal.y*cast(double)p.y+cast(double)normal.z*cast(double)p.z-cast(double)w);
3444 // distance from point to plane
3445 // plane must be normalized
3446 double dbldistance() (in auto ref vec3 p) const {
3447 pragma(inline, true);
3448 return (cast(double)normal.x*p.x+cast(double)normal.y*p.y+cast(double)normal.z*cast(double)p.z)/normal.dbllength;
3451 // distance from point to plane
3452 // plane must be normalized
3453 Float distance() (in auto ref vec3 p) const {
3454 pragma(inline, true);
3455 return cast(Float)dbldistance;
3458 // "land" point onto the plane
3459 // plane must be normalized
3460 vec3 landAlongNormal() (in auto ref vec3 p) const {
3461 pragma(inline, true);
3462 mixin(ImportCoreMath!(Float, "fabs"));
3463 immutable double pdist = pointSideF(p);
3464 return (fabs(pdist) > EPSILON!Float ? p-normal*pdist : p);
3467 // "land" point onto the plane
3468 // plane must be normalized
3470 vec3 land() (in auto ref vec3 p) const {
3471 mixin(ImportCoreMath!(double, "fabs"));
3472 // distance from point to plane
3473 double pdist = (cast(double)normal.x*p.x+cast(double)normal.y*p.y+cast(double)normal.z*cast(double)p.z)/normal.dbllength;
3474 // just-in-case check
3475 if (fabs(pdist) > EPSILON!double) {
3476 // get side
3477 return p+normal*pdist;
3478 } else {
3479 // on the plane
3480 return p;
3485 // plane must be normalized
3486 vec3 project() (in auto ref vec3 v) const {
3487 pragma(inline, true);
3488 mixin(ImportCoreMath!(double, "fabs"));
3489 return v-(v-normal*w).dot(normal)*normal;
3492 // returns the point where the line p0-p1 intersects this plane
3493 vec3 lineIntersect() (in auto ref vec3 p0, in auto ref vec3 p1) const {
3494 pragma(inline, true);
3495 immutable dif = p1-p0;
3496 immutable t = (w-normal.dot(p0))/normal.dot(dif);
3497 return p0+(dif*t);
3500 // swizzling
3501 static if (enableSwizzling) auto opDispatch(string fld) () if (isGoodSwizzling!(fld, "xyzw", 2, 3)) {
3502 static if (fld.length == 2) {
3503 return mixin(SwizzleCtor!("vec2", fld));
3504 } else {
3505 return mixin(SwizzleCtor!("vec3", fld));
3509 // sweep test; if `true` (hit), `hitpos` will be sphere position when it hits this plane, and `u` will be normalized collision time
3510 bool sweep(ST) (in auto ref ST sphere, in auto ref vec3 amove, vec3* hitpos, Float* u) const if (IsSphere!(ST, vec3)) {
3511 mixin(ImportCoreMath!(Float, "fabs"));
3512 immutable c0 = sphere.orig;
3513 immutable c1 = c0+amove;
3514 immutable Float r = sphere.radius;
3515 immutable Float d0 = (normal*c0)+w;
3516 immutable Float d1 = (normal*c1)+w;
3517 // check if the sphere is touching the plane
3518 if (fabs(d0) <= r) {
3519 if (hitpos !is null) *hitpos = c0;
3520 if (u !is null) *u = 0;
3521 return true;
3523 // check if the sphere penetrated during movement
3524 if (d0 > r && d1 < r) {
3525 immutable Float uu = (d0-r)/(d0-d1); // normalized time
3526 if (u !is null) *u = uu;
3527 if (hitpos !is null) *hitpos = (1-uu)*c0+uu*c1; // point of first contact
3528 return true;
3530 // no collision
3531 return false;
3534 // intersection of 3 planes, Graphics Gems 1 pg 305
3535 auto intersectionPoint() (in auto ref plane3 plane2, in auto ref plane3 plane3) const {
3536 mixin(ImportCoreMath!(Float, "fabs"));
3537 alias plane1 = this;
3538 immutable Float det = plane1.normal.cross(plane2.normal).dot(plane3.normal);
3539 // if the determinant is 0, that means parallel planes, no intersection
3540 if (fabs(det) < EPSILON!Float) return vec3.Invalid;
3541 return
3542 (plane2.normal.cross(plane3.normal)*(-plane1.w)+
3543 plane3.normal.cross(plane1.normal)*(-plane2.w)+
3544 plane1.normal.cross(plane2.normal)*(-plane3.w))/det;
3549 // ////////////////////////////////////////////////////////////////////////// //
3550 struct Ray(VT) if (IsVector!VT) {
3551 public:
3552 alias vec = VT;
3553 alias Float = VT.Float;
3555 public:
3556 VT orig, dir; // dir should be normalized (setters does this)
3558 nothrow @safe:
3559 string toString () const {
3560 import std.format : format;
3561 try {
3562 return "[(%s,%s):(%s,%s)]".format(orig.x, orig.y, dir.x, dir.y);
3563 } catch (Exception) {
3564 assert(0);
3568 @nogc:
3569 this (VT.Float x, VT.Float y, VT.Float angle) { pragma(inline, true); setOrigDir(x, y, angle); }
3570 this() (in auto ref VT aorg, VT.Float angle) { pragma(inline, true); setOrigDir(aorg, angle); }
3572 static Ray!VT fromPoints() (in auto ref VT p0, in auto ref VT p1) {
3573 pragma(inline, true);
3574 Ray!VT res;
3575 res.orig = p0;
3576 res.dir = (p1-p0).normalized;
3577 return res;
3580 static if (VT.Dims == 2) void setOrigDir (VT.Float x, VT.Float y, VT.Float angle) {
3581 pragma(inline, true);
3582 mixin(ImportCoreMath!(Float, "cos", "sin"));
3583 orig.x = x;
3584 orig.y = y;
3585 dir.x = cos(angle);
3586 dir.y = sin(angle);
3589 static if (VT.Dims == 2) void setOrigDir() (in auto ref VT aorg, VT.Float angle) {
3590 pragma(inline, true);
3591 mixin(ImportCoreMath!(Float, "cos", "sin"));
3592 orig.x = aorg.x;
3593 orig.y = aorg.y;
3594 dir.x = cos(angle);
3595 dir.y = sin(angle);
3598 static if (VT.Dims == 2) void setOrig (VT.Float x, VT.Float y) {
3599 pragma(inline, true);
3600 orig.x = x;
3601 orig.y = y;
3604 void setOrig() (in auto ref VT aorg) {
3605 pragma(inline, true);
3606 orig.x = aorg.x;
3607 orig.y = aorg.y;
3608 static if (VT.Dims == 3) orig.z = aorg.z;
3611 static if (VT.Dims == 2) void setDir (VT.Float angle) {
3612 pragma(inline, true);
3613 mixin(ImportCoreMath!(Float, "cos", "sin"));
3614 dir.x = cos(angle);
3615 dir.y = sin(angle);
3618 @property VT right () const {
3619 pragma(inline, true);
3620 static if (VT.Dims == 2) return VT(dir.y, -dir.x);
3621 else return VT(dir.y, -dir.x, dir.z);
3624 VT pointAt (VT.Float len) const { pragma(inline, true); return orig+dir*len; }
3628 // ////////////////////////////////////////////////////////////////////////// //
3629 public struct AABBImpl(VT) if (IsVector!VT) {
3630 private:
3631 static T nmin(T) (in T a, in T b) { pragma(inline, true); return (a < b ? a : b); }
3632 static T nmax(T) (in T a, in T b) { pragma(inline, true); return (a > b ? a : b); }
3634 public:
3635 alias VType = VT;
3636 alias Float = VT.Float;
3637 alias Me = typeof(this);
3639 public:
3640 //VT center;
3641 //VT half; // should be positive
3642 VT min, max;
3644 public:
3645 string toString () const {
3646 import std.format : format;
3647 return "[%s-%s]".format(min, max);
3650 public nothrow @safe @nogc:
3651 this() (in auto ref VT amin, in auto ref VT amax) {
3652 pragma(inline, true);
3653 //center = (amin+amax)/2;
3654 //half = VT((nmax(amin.x, amax.x)-nmin(amin.x, amax.x))/2, (nmax(amin.y, amax.y)-nmin(amin.y, amax.y))/2);
3655 version(none) {
3656 // this breaks VecN ctor inliner (fuck!)
3657 static if (VT.Dims == 2) {
3658 min = VT(nmin(amin.x, amax.x), nmin(amin.y, amax.y));
3659 max = VT(nmax(amin.x, amax.x), nmax(amin.y, amax.y));
3660 } else {
3661 min = VT(nmin(amin.x, amax.x), nmin(amin.y, amax.y), nmin(amin.z, amax.z));
3662 max = VT(nmax(amin.x, amax.x), nmax(amin.y, amax.y), nmax(amin.z, amax.z));
3664 } else {
3665 min.x = nmin(amin.x, amax.x);
3666 min.y = nmin(amin.y, amax.y);
3667 static if (VT.Dims == 3) min.z = nmin(amin.z, amax.z);
3668 max.x = nmax(amin.x, amax.x);
3669 max.y = nmax(amin.y, amax.y);
3670 static if (VT.Dims == 3) max.z = nmax(amin.z, amax.z);
3674 void reset () {
3675 static if (VT.Dims == 2) {
3676 min = VT(+VT.Float.infinity, +VT.Float.infinity);
3677 max = VT(-VT.Float.infinity, -VT.Float.infinity);
3678 } else {
3679 min = VT(+VT.Float.infinity, +VT.Float.infinity, +VT.Float.infinity);
3680 max = VT(-VT.Float.infinity, -VT.Float.infinity, -VT.Float.infinity);
3684 void setMinMax() (in auto ref VT amin, in auto ref VT amax) pure {
3685 pragma(inline, true);
3686 version(none) {
3687 // this breaks VecN ctor inliner (fuck!)
3688 static if (VT.Dims == 2) {
3689 min = VT(nmin(amin.x, amax.x), nmin(amin.y, amax.y));
3690 max = VT(nmax(amin.x, amax.x), nmax(amin.y, amax.y));
3691 } else {
3692 min = VT(nmin(amin.x, amax.x), nmin(amin.y, amax.y), nmin(amin.z, amax.z));
3693 max = VT(nmax(amin.x, amax.x), nmax(amin.y, amax.y), nmax(amin.z, amax.z));
3695 } else {
3696 min.x = nmin(amin.x, amax.x);
3697 min.y = nmin(amin.y, amax.y);
3698 static if (VT.Dims == 3) min.z = nmin(amin.z, amax.z);
3699 max.x = nmax(amin.x, amax.x);
3700 max.y = nmax(amin.y, amax.y);
3701 static if (VT.Dims == 3) max.z = nmax(amin.z, amax.z);
3705 //@property VT min () const pure { pragma(inline, true); return center-half; }
3706 //@property VT max () const pure { pragma(inline, true); return center+half; }
3708 VT center () const pure { pragma(inline, true); return (min+max)/2; }
3709 VT extent () const pure { pragma(inline, true); return max-min; }
3711 //@property valid () const { pragma(inline, true); return center.isFinite && half.isFinite; }
3712 @property valid () const {
3713 pragma(inline, true);
3714 static if (VT.Dims == 2) {
3715 return (min.isFinite && max.isFinite && min.x <= max.x && min.y <= max.y);
3716 } else {
3717 return (min.isFinite && max.isFinite && min.x <= max.x && min.y <= max.y && min.z <= max.z);
3721 // return the volume of the AABB
3722 @property Float volume () const {
3723 pragma(inline, true);
3724 immutable diff = max-min;
3725 static if (VT.Dims == 3) {
3726 return diff.x*diff.y*diff.z;
3727 } else {
3728 return diff.x*diff.y;
3732 static auto mergeAABBs() (in auto ref Me aabb1, in auto ref Me aabb2) {
3733 typeof(this) res;
3734 res.merge(aabb1, aabb2);
3735 return res;
3738 void merge() (in auto ref Me aabb1, in auto ref Me aabb2) {
3739 pragma(inline, true);
3740 min.x = nmin(aabb1.min.x, aabb2.min.x);
3741 min.y = nmin(aabb1.min.y, aabb2.min.y);
3742 max.x = nmax(aabb1.max.x, aabb2.max.x);
3743 max.y = nmax(aabb1.max.y, aabb2.max.y);
3744 static if (VT.Dims == 3) {
3745 min.z = nmin(aabb1.min.z, aabb2.min.z);
3746 max.z = nmax(aabb1.max.z, aabb2.max.z);
3750 void merge() (in auto ref Me aabb1) {
3751 pragma(inline, true);
3752 min.x = nmin(aabb1.min.x, min.x);
3753 min.y = nmin(aabb1.min.y, min.y);
3754 max.x = nmax(aabb1.max.x, max.x);
3755 max.y = nmax(aabb1.max.y, max.y);
3756 static if (VT.Dims == 3) {
3757 min.z = nmin(aabb1.min.z, min.z);
3758 max.z = nmax(aabb1.max.z, max.z);
3762 void addPoint() (in auto ref VT v) {
3763 min.x = nmin(min.x, v.x);
3764 max.x = nmax(max.x, v.x);
3765 min.y = nmin(min.y, v.y);
3766 max.y = nmax(max.y, v.y);
3767 static if (VT.Dims == 3) {
3768 min.z = nmin(min.z, v.z);
3769 max.z = nmax(max.z, v.z);
3773 void opOpAssign(string op:"~") (in auto ref VT v) { pragma(inline, true); addPoint(v); }
3774 void opOpAssign(string op:"~") (in auto ref Me aabb1) { pragma(inline, true); merge(aabb1); }
3776 // return true if the current AABB contains the AABB given in parameter
3777 bool contains() (in auto ref Me aabb) const {
3778 //pragma(inline, true);
3779 static if (VT.Dims == 3) {
3780 return
3781 aabb.min.x >= min.x && aabb.min.y >= min.y && aabb.min.z >= min.z &&
3782 aabb.max.x <= max.x && aabb.max.y <= max.y && aabb.max.z <= max.z;
3783 } else {
3784 return
3785 aabb.min.x >= min.x && aabb.min.y >= min.y &&
3786 aabb.max.x <= max.x && aabb.max.y <= max.y;
3790 bool contains() (in auto ref VT p) const {
3791 pragma(inline, true);
3792 static if (VT.Dims == 2) {
3793 return (p.x >= min.x && p.y >= min.y && p.x <= max.x && p.y <= max.y);
3794 } else {
3795 return (p.x >= min.x && p.y >= min.y && p.z >= min.z && p.x <= max.x && p.y <= max.y && p.z <= max.z);
3799 // extrude bbox a little, to compensate floating point inexactness
3801 void extrude (Float delta) pure {
3802 min.x -= delta;
3803 min.y -= delta;
3804 static if (VT.Dims == 3) min.z -= delta;
3805 max.x += delta;
3806 max.y += delta;
3807 static if (VT.Dims == 3) max.z += delta;
3811 // return true if the current AABB is overlapping with the AABB in parameter
3812 // two AABBs overlap if they overlap in the two(three) x, y (and z) axes at the same time
3813 bool overlaps() (in auto ref Me aabb) const {
3814 //pragma(inline, true);
3815 // exit with no intersection if found separated along any axis
3816 if (max.x < aabb.min.x || min.x > aabb.max.x) return false;
3817 if (max.y < aabb.min.y || min.y > aabb.max.y) return false;
3818 static if (VT.Dims == 3) {
3819 if (max.z < aabb.min.z || min.z > aabb.max.z) return false;
3821 return true;
3824 // ////////////////////////////////////////////////////////////////////////// //
3825 // something to consider here is that 0 * inf =nan which occurs when the ray starts exactly on the edge of a box
3826 // rd: ray direction, normalized
3827 // https://tavianator.com/fast-branchless-raybounding-box-intersections-part-2-nans/
3828 static bool intersects() (in auto ref VT bmin, in auto ref VT bmax, in auto ref Ray!VT ray, Float* tmino=null, Float* tmaxo=null) {
3829 // ok with coplanars, but dmd sux at unrolled loops
3830 // do X
3831 immutable Float dinvp0 = cast(Float)1/ray.dir.x; // 1/0 will produce inf
3832 immutable Float t1p0 = (bmin.x-ray.orig.x)*dinvp0;
3833 immutable Float t2p0 = (bmax.x-ray.orig.x)*dinvp0;
3834 Float tmin = nmin(t1p0, t2p0);
3835 Float tmax = nmax(t1p0, t2p0);
3836 // do Y
3838 immutable Float dinv = cast(Float)1/ray.dir.y; // 1/0 will produce inf
3839 immutable Float t1 = (bmin.y-ray.orig.y)*dinv;
3840 immutable Float t2 = (bmax.y-ray.orig.y)*dinv;
3841 tmin = nmax(tmin, nmin(nmin(t1, t2), tmax));
3842 tmax = nmin(tmax, nmax(nmax(t1, t2), tmin));
3844 // do Z
3845 static if (VT.Dims == 3) {
3847 immutable Float dinv = cast(Float)1/ray.dir.z; // 1/0 will produce inf
3848 immutable Float t1 = (bmin.z-ray.orig.z)*dinv;
3849 immutable Float t2 = (bmax.z-ray.orig.z)*dinv;
3850 tmin = nmax(tmin, nmin(nmin(t1, t2), tmax));
3851 tmax = nmin(tmax, nmax(nmax(t1, t2), tmin));
3854 if (tmax > nmax(tmin, cast(Float)0)) {
3855 if (tmino !is null) *tmino = tmin;
3856 if (tmaxo !is null) *tmaxo = tmax;
3857 return true;
3858 } else {
3859 return false;
3863 bool intersects() (in auto ref Ray!VT ray, Float* tmino=null, Float* tmaxo=null) const @trusted {
3864 // ok with coplanars, but dmd sux at unrolled loops
3865 // do X
3866 immutable Float dinvp0 = cast(Float)1/ray.dir.x; // 1/0 will produce inf
3867 immutable Float t1p0 = (min.x-ray.orig.x)*dinvp0;
3868 immutable Float t2p0 = (max.x-ray.orig.x)*dinvp0;
3869 Float tmin = nmin(t1p0, t2p0);
3870 Float tmax = nmax(t1p0, t2p0);
3871 // do Y
3873 immutable Float dinv = cast(Float)1/ray.dir.y; // 1/0 will produce inf
3874 immutable Float t1 = (min.y-ray.orig.y)*dinv;
3875 immutable Float t2 = (max.y-ray.orig.y)*dinv;
3876 tmin = nmax(tmin, nmin(nmin(t1, t2), tmax));
3877 tmax = nmin(tmax, nmax(nmax(t1, t2), tmin));
3879 // do Z
3880 static if (VT.Dims == 3) {
3882 immutable Float dinv = cast(Float)1/ray.dir.z; // 1/0 will produce inf
3883 immutable Float t1 = (min.z-ray.orig.z)*dinv;
3884 immutable Float t2 = (max.z-ray.orig.z)*dinv;
3885 tmin = nmax(tmin, nmin(nmin(t1, t2), tmax));
3886 tmax = nmin(tmax, nmax(nmax(t1, t2), tmin));
3889 if (tmax > nmax(tmin, cast(Float)0)) {
3890 if (tmino !is null) *tmino = tmin;
3891 if (tmaxo !is null) *tmaxo = tmax;
3892 return true;
3893 } else {
3894 return false;
3898 Float segIntersectMin() (in auto ref VT a, in auto ref VT b) const @trusted {
3899 Float tmin;
3900 if (!intersects(Ray!VT.fromPoints(a, b), &tmin)) return -1;
3901 if (tmin < 0) return 0; // inside
3902 if (tmin*tmin > (b-a).lengthSquared) return -1;
3903 return tmin;
3906 Float segIntersectMax() (in auto ref VT a, in auto ref VT b) const @trusted {
3907 Float tmax;
3908 if (!intersects(Ray!VT.fromPoints(a, b), null, &tmax)) return -1;
3909 if (tmax*tmax > (b-a).lengthSquared) return -1;
3910 return tmax;
3913 bool isIntersects() (in auto ref VT a, in auto ref VT b) const @trusted {
3914 // it may be faster to first check if start or end point is inside AABB (this is sometimes enough for dyntree)
3915 static if (VT.Dims == 2) {
3916 if (a.x >= min.x && a.y >= min.y && a.x <= max.x && a.y <= max.y) return true; // a
3917 if (b.x >= min.x && b.y >= min.y && b.x <= max.x && b.y <= max.y) return true; // b
3918 } else {
3919 if (a.x >= min.x && a.y >= min.y && a.z >= min.z && a.x <= max.x && a.y <= max.y && a.z <= max.z) return true; // a
3920 if (b.x >= min.x && b.y >= min.y && b.z >= min.z && b.x <= max.x && b.y <= max.y && b.z <= max.z) return true; // b
3922 // nope, do it hard way
3923 Float tmin;
3924 if (!intersects(Ray!VT.fromPoints(a, b), &tmin)) return false;
3925 if (tmin < 0) return true; // inside, just in case
3926 return (tmin*tmin <= (b-a).lengthSquared);
3929 ref inout(VT) opIndex (usize idx) inout {
3930 pragma(inline, true);
3931 return (idx == 0 ? min : max);
3934 /// sweep two AABB's to see if and when they are overlapping
3935 /// returns `true` if collision was detected (or boxes overlaps)
3936 /// u0 = normalized time of first collision (i.e. collision starts at myMove*u0)
3937 /// u1 = normalized time of second collision (i.e. collision stops after myMove*u1)
3938 /// hitnormal = normal that will move `this` apart of `b` edge it collided with
3939 /// no output values are valid if no collision was detected
3940 /// WARNING! hit normal calculation is not tested!
3941 bool sweep() (in auto ref VT myMove, in auto ref Me b, Float* u0, VT* hitnormal=null, Float* u1=null) const @trusted {
3942 // check if they are overlapping right now
3943 if (this.overlaps(b)) {
3944 if (u0 !is null) *u0 = 0;
3945 if (u1 !is null) *u1 = 0;
3946 if (hitnormal !is null) *hitnormal = VT(); // oops
3947 return true;
3950 immutable v = -myMove; // treat b as stationary, so invert v to get relative velocity
3952 // not moving, and not overlapping
3953 if (v.isZero) return false;
3955 Float hitTime = 0;
3956 Float outTime = 1;
3957 Float[VT.Dims] overlapTime = 0;
3959 alias a = this;
3960 foreach (immutable aidx; 0..VT.Dims) {
3961 // axis overlap
3962 immutable Float vv = v[aidx];
3963 if (vv < 0) {
3964 immutable Float invv = cast(Float)1/vv;
3965 if (b.max[aidx] < a.min[aidx]) return false;
3966 if (b.max[aidx] > a.min[aidx]) outTime = nmin((a.min[aidx]-b.max[aidx])*invv, outTime);
3967 if (a.max[aidx] < b.min[aidx]) hitTime = nmax((overlapTime.ptr[aidx] = (a.max[aidx]-b.min[aidx])*invv), hitTime);
3968 if (hitTime > outTime) return false;
3969 } else if (vv > 0) {
3970 immutable Float invv = cast(Float)1/vv;
3971 if (b.min[aidx] > a.max[aidx]) return false;
3972 if (a.max[aidx] > b.min[aidx]) outTime = nmin((a.max[aidx]-b.min[aidx])*invv, outTime);
3973 if (b.max[aidx] < a.min[aidx]) hitTime = nmax((overlapTime.ptr[aidx] = (a.min[aidx]-b.max[aidx])*invv), hitTime);
3974 if (hitTime > outTime) return false;
3978 if (u0 !is null) *u0 = hitTime;
3979 if (u1 !is null) *u1 = outTime;
3981 // hit normal is along axis with the highest overlap time
3982 if (hitnormal !is null) {
3983 static if (VT.Dims == 3) {
3984 int aidx = 0;
3985 if (overlapTime.ptr[1] > overlapTime.ptr[0]) aidx = 1;
3986 if (overlapTime.ptr[2] > overlapTime.ptr[aidx]) aidx = 2;
3987 VT hn; // zero vector
3988 hn[aidx] = (v[aidx] < 0 ? -1 : v[aidx] > 0 ? 1 : 0);
3989 *hitnormal = hn;
3990 } else {
3991 if (overlapTime.ptr[0] > overlapTime.ptr[1]) {
3992 *hitnormal = VT((v.x < 0 ? -1 : v.x > 0 ? 1 : 0), 0);
3993 } else {
3994 *hitnormal = VT(0, (v.y < 0 ? -1 : v.y > 0 ? 1 : 0));
3999 return true;
4002 version(none) {
4003 auto u_0 = VT(0, 0, 0); // first times of overlap along each axis
4004 auto u_1 = VT(1, 1, 1); // last times of overlap along each axis
4005 bool wasHit = false;
4007 // find the possible first and last times of overlap along each axis
4008 foreach (immutable idx; 0..VT.Dims) {
4009 Float dinv = v[idx];
4010 if (dinv != 0) {
4011 dinv = cast(Float)1/dinv;
4012 if (this.max[idx] < b.min[idx] && dinv < 0) {
4013 u_0[idx] = (this.max[idx]-b.min[idx])*dinv;
4014 wasHit = true;
4015 } else if (b.max[idx] < this.min[idx] && dinv > 0) {
4016 u_0[idx] = (this.min[idx]-b.max[idx])*dinv;
4017 wasHit = true;
4019 if (b.max[idx] > this.min[idx] && dinv < 0) {
4020 u_1[idx] = (this.min[idx]-b.max[idx])*dinv;
4021 wasHit = true;
4022 } else if (this.max[idx] > b.min[idx] && dinv > 0) {
4023 u_1[idx] = (this.max[idx]-b.min[idx])*dinv;
4024 wasHit = true;
4029 // oops
4030 if (!wasHit) {
4031 if (u0 !is null) *u0 = 0;
4032 if (u1 !is null) *u1 = 0;
4033 return false;
4036 static if (VT.Dims == 3) {
4037 immutable Float uu0 = nmax(u_0.x, nmax(u_0.y, u_0.z)); // possible first time of overlap
4038 immutable Float uu1 = nmin(u_1.x, nmin(u_1.y, u_1.z)); // possible last time of overlap
4039 } else {
4040 immutable Float uu0 = nmax(u_0.x, u_0.y); // possible first time of overlap
4041 immutable Float uu1 = nmin(u_1.x, u_1.y); // possible last time of overlap
4044 if (u0 !is null) *u0 = uu0;
4045 if (u1 !is null) *u1 = uu1;
4047 // they could have only collided if the first time of overlap occurred before the last time of overlap
4048 return (uu0 <= uu1);
4053 /// check to see if the sphere overlaps the AABB
4054 bool overlaps(ST) (in auto ref ST sphere) if (IsSphere!(ST, VT)) { pragma(inline, true); return overlapsSphere(sphere.orig, sphere.radius); }
4056 /// check to see if the sphere overlaps the AABB
4057 bool overlapsSphere() (in auto ref VT center, Float radius) {
4058 Float d = 0;
4059 // find the square of the distance from the sphere to the box
4060 foreach (immutable idx; 0..VT.Dims) {
4061 if (center[idx] < min[idx]) {
4062 immutable Float s = center[idx]-min[idx];
4063 d += s*s;
4064 } else if (center[idx] > max[idx]) {
4065 immutable Float s = center[idx]-max[idx];
4066 d += s*s;
4069 return (d <= radius*radius);
4074 // ////////////////////////////////////////////////////////////////////////// //
4075 /* Dynamic AABB tree (bounding volume hierarchy)
4076 * based on the code from ReactPhysics3D physics library, http://www.reactphysics3d.com
4077 * Copyright (c) 2010-2016 Daniel Chappuis
4079 * This software is provided 'as-is', without any express or implied warranty.
4080 * In no event will the authors be held liable for any damages arising from the
4081 * use of this software.
4083 * Permission is granted to anyone to use this software for any purpose,
4084 * including commercial applications, and to alter it and redistribute it
4085 * freely, subject to the following restrictions:
4087 * 1. The origin of this software must not be misrepresented; you must not claim
4088 * that you wrote the original software. If you use this software in a
4089 * product, an acknowledgment in the product documentation would be
4090 * appreciated but is not required.
4092 * 2. Altered source versions must be plainly marked as such, and must not be
4093 * misrepresented as being the original software.
4095 * 3. This notice may not be removed or altered from any source distribution.
4097 /* WARNING! BY DEFAULT TREE WILL NOT PROTECT OBJECTS FROM GC! */
4098 // ////////////////////////////////////////////////////////////////////////// //
4100 * This class implements a dynamic AABB tree that is used for broad-phase
4101 * collision detection. This data structure is inspired by Nathanael Presson's
4102 * dynamic tree implementation in BulletPhysics. The following implementation is
4103 * based on the one from Erin Catto in Box2D as described in the book
4104 * "Introduction to Game Physics with Box2D" by Ian Parberry.
4106 /// Dynamic AABB Tree: can be used to speed up broad phase in various engines
4107 /// GCAnchor==true: add nodes as GC roots; you won't need it if you're storing nodes in some other way
4108 public final class DynamicAABBTree(VT, BodyBase, bool GCAnchor=false) if (IsVector!VT && is(BodyBase == class)) {
4109 private:
4110 static T min(T) (in T a, in T b) { pragma(inline, true); return (a < b ? a : b); }
4111 static T max(T) (in T a, in T b) { pragma(inline, true); return (a > b ? a : b); }
4113 public:
4114 alias VType = VT;
4115 alias Float = VT.Float;
4116 alias Me = typeof(this);
4117 alias AABB = AABBImpl!VT;
4119 enum FloatNum(Float v) = cast(Float)v;
4121 public:
4122 // in the broad-phase collision detection (dynamic AABB tree), the AABBs are
4123 // also inflated in direction of the linear motion of the body by mutliplying the
4124 // followin constant with the linear velocity and the elapsed time between two frames
4125 enum Float LinearMotionGapMultiplier = FloatNum!(1.7);
4127 public:
4128 // called when a overlapping node has been found during the call to forEachAABBOverlap()
4129 // return `true` to stop
4130 alias OverlapCallback = void delegate (BodyBase abody);
4131 alias SimpleQueryCallback = bool delegate (BodyBase abody);
4132 alias SimpleQueryCallbackNR = void delegate (BodyBase abody);
4133 alias SegQueryCallback = Float delegate (BodyBase abody, in ref VT a, in ref VT b); // return dist from a to abody
4135 private:
4136 align(1) struct TreeNode {
4137 align(1):
4138 enum NullTreeNode = -1;
4139 enum { Left = 0, Right = 1 }
4140 // a node is either in the tree (has a parent) or in the free nodes list (has a next node)
4141 union {
4142 int parentId;
4143 int nextNodeId;
4145 // a node is either a leaf (has data) or is an internal node (has children)
4146 union {
4147 int[2] children; /// left and right child of the node (children[0] = left child)
4148 BodyBase flesh;
4150 // height of the node in the tree (-1 for free nodes)
4151 short height;
4152 // fat axis aligned bounding box (AABB) corresponding to the node
4153 AABBImpl!VT aabb;
4154 // return true if the node is a leaf of the tree
4155 @property const pure nothrow @safe @nogc {
4156 bool leaf () { pragma(inline, true); return (height == 0); }
4157 bool free () { pragma(inline, true); return (height == -1); }
4161 private:
4162 TreeNode* mNodes; // pointer to the memory location of the nodes of the tree
4163 int mRootNodeId; // id of the root node of the tree
4164 int mFreeNodeId; // id of the first node of the list of free (allocated) nodes in the tree that we can use
4165 int mAllocCount; // number of allocated nodes in the tree
4166 int mNodeCount; // number of nodes in the tree
4168 // extra AABB Gap used to allow the collision shape to move a little bit
4169 // without triggering a large modification of the tree which can be costly
4170 Float mExtraGap;
4172 private:
4173 // allocate and return a node to use in the tree
4174 int allocateNode () {
4175 // if there is no more allocated node to use
4176 if (mFreeNodeId == TreeNode.NullTreeNode) {
4177 import core.stdc.stdlib : realloc;
4178 version(aabbtree_many_asserts) assert(mNodeCount == mAllocCount);
4179 // allocate more nodes in the tree
4180 auto newsz = (mAllocCount < 8192 ? mAllocCount*2 : mAllocCount+8192);
4181 TreeNode* nn = cast(TreeNode*)realloc(mNodes, newsz*TreeNode.sizeof);
4182 if (nn is null) assert(0, "out of memory");
4183 //{ import core.stdc.stdio; printf("realloced: old=%u; new=%u\n", mAllocCount, newsz); }
4184 mAllocCount = newsz;
4185 mNodes = nn;
4186 // initialize the allocated nodes
4187 foreach (int i; mNodeCount..mAllocCount-1) {
4188 mNodes[i].nextNodeId = i+1;
4189 mNodes[i].height = -1;
4191 mNodes[mAllocCount-1].nextNodeId = TreeNode.NullTreeNode;
4192 mNodes[mAllocCount-1].height = -1;
4193 mFreeNodeId = mNodeCount;
4195 // get the next free node
4196 int freeNodeId = mFreeNodeId;
4197 version(aabbtree_many_asserts) assert(freeNodeId < mAllocCount);
4198 mFreeNodeId = mNodes[freeNodeId].nextNodeId;
4199 mNodes[freeNodeId].parentId = TreeNode.NullTreeNode;
4200 mNodes[freeNodeId].height = 0;
4201 ++mNodeCount;
4202 return freeNodeId;
4205 // release a node
4206 void releaseNode (int nodeId) {
4207 version(aabbtree_many_asserts) assert(mNodeCount > 0);
4208 version(aabbtree_many_asserts) assert(nodeId >= 0 && nodeId < mAllocCount);
4209 version(aabbtree_many_asserts) assert(mNodes[nodeId].height >= 0);
4210 mNodes[nodeId].nextNodeId = mFreeNodeId;
4211 mNodes[nodeId].height = -1;
4212 mFreeNodeId = nodeId;
4213 --mNodeCount;
4216 // insert a leaf node in the tree
4217 // the process of inserting a new leaf node in the dynamic tree is described in the book "Introduction to Game Physics with Box2D" by Ian Parberry
4218 void insertLeafNode (int nodeId) {
4219 // if the tree is empty
4220 if (mRootNodeId == TreeNode.NullTreeNode) {
4221 mRootNodeId = nodeId;
4222 mNodes[mRootNodeId].parentId = TreeNode.NullTreeNode;
4223 return;
4226 version(aabbtree_many_asserts) assert(mRootNodeId != TreeNode.NullTreeNode);
4228 // find the best sibling node for the new node
4229 AABB newNodeAABB = mNodes[nodeId].aabb;
4230 int currentNodeId = mRootNodeId;
4231 while (!mNodes[currentNodeId].leaf) {
4232 int leftChild = mNodes[currentNodeId].children.ptr[TreeNode.Left];
4233 int rightChild = mNodes[currentNodeId].children.ptr[TreeNode.Right];
4235 // compute the merged AABB
4236 Float volumeAABB = mNodes[currentNodeId].aabb.volume;
4237 AABB mergedAABBs = AABB.mergeAABBs(mNodes[currentNodeId].aabb, newNodeAABB);
4238 Float mergedVolume = mergedAABBs.volume;
4240 // compute the cost of making the current node the sibling of the new node
4241 Float costS = FloatNum!2*mergedVolume;
4243 // compute the minimum cost of pushing the new node further down the tree (inheritance cost)
4244 Float costI = FloatNum!2*(mergedVolume-volumeAABB);
4246 // compute the cost of descending into the left child
4247 AABB currentAndLeftAABB = AABB.mergeAABBs(newNodeAABB, mNodes[leftChild].aabb);
4248 Float costLeft = currentAndLeftAABB.volume+costI;
4249 if (!mNodes[leftChild].leaf) costLeft -= mNodes[leftChild].aabb.volume;
4251 // compute the cost of descending into the right child
4252 AABB currentAndRightAABB = AABB.mergeAABBs(newNodeAABB, mNodes[rightChild].aabb);
4253 Float costRight = currentAndRightAABB.volume+costI;
4254 if (!mNodes[rightChild].leaf) costRight -= mNodes[rightChild].aabb.volume;
4256 // if the cost of making the current node a sibling of the new node is smaller than the cost of going down into the left or right child
4257 if (costS < costLeft && costS < costRight) break;
4259 // it is cheaper to go down into a child of the current node, choose the best child
4260 currentNodeId = (costLeft < costRight ? leftChild : rightChild);
4263 int siblingNode = currentNodeId;
4265 // create a new parent for the new node and the sibling node
4266 int oldParentNode = mNodes[siblingNode].parentId;
4267 int newParentNode = allocateNode();
4268 mNodes[newParentNode].parentId = oldParentNode;
4269 mNodes[newParentNode].aabb.merge(mNodes[siblingNode].aabb, newNodeAABB);
4270 mNodes[newParentNode].height = cast(short)(mNodes[siblingNode].height+1);
4271 version(aabbtree_many_asserts) assert(mNodes[newParentNode].height > 0);
4273 // if the sibling node was not the root node
4274 if (oldParentNode != TreeNode.NullTreeNode) {
4275 version(aabbtree_many_asserts) assert(!mNodes[oldParentNode].leaf);
4276 if (mNodes[oldParentNode].children.ptr[TreeNode.Left] == siblingNode) {
4277 mNodes[oldParentNode].children.ptr[TreeNode.Left] = newParentNode;
4278 } else {
4279 mNodes[oldParentNode].children.ptr[TreeNode.Right] = newParentNode;
4281 mNodes[newParentNode].children.ptr[TreeNode.Left] = siblingNode;
4282 mNodes[newParentNode].children.ptr[TreeNode.Right] = nodeId;
4283 mNodes[siblingNode].parentId = newParentNode;
4284 mNodes[nodeId].parentId = newParentNode;
4285 } else {
4286 // if the sibling node was the root node
4287 mNodes[newParentNode].children.ptr[TreeNode.Left] = siblingNode;
4288 mNodes[newParentNode].children.ptr[TreeNode.Right] = nodeId;
4289 mNodes[siblingNode].parentId = newParentNode;
4290 mNodes[nodeId].parentId = newParentNode;
4291 mRootNodeId = newParentNode;
4294 // move up in the tree to change the AABBs that have changed
4295 currentNodeId = mNodes[nodeId].parentId;
4296 version(aabbtree_many_asserts) assert(!mNodes[currentNodeId].leaf);
4297 while (currentNodeId != TreeNode.NullTreeNode) {
4298 // balance the sub-tree of the current node if it is not balanced
4299 currentNodeId = balanceSubTreeAtNode(currentNodeId);
4300 version(aabbtree_many_asserts) assert(mNodes[nodeId].leaf);
4302 version(aabbtree_many_asserts) assert(!mNodes[currentNodeId].leaf);
4303 int leftChild = mNodes[currentNodeId].children.ptr[TreeNode.Left];
4304 int rightChild = mNodes[currentNodeId].children.ptr[TreeNode.Right];
4305 version(aabbtree_many_asserts) assert(leftChild != TreeNode.NullTreeNode);
4306 version(aabbtree_many_asserts) assert(rightChild != TreeNode.NullTreeNode);
4308 // recompute the height of the node in the tree
4309 mNodes[currentNodeId].height = cast(short)(max(mNodes[leftChild].height, mNodes[rightChild].height)+1);
4310 version(aabbtree_many_asserts) assert(mNodes[currentNodeId].height > 0);
4312 // recompute the AABB of the node
4313 mNodes[currentNodeId].aabb.merge(mNodes[leftChild].aabb, mNodes[rightChild].aabb);
4315 currentNodeId = mNodes[currentNodeId].parentId;
4318 version(aabbtree_many_asserts) assert(mNodes[nodeId].leaf);
4321 // remove a leaf node from the tree
4322 void removeLeafNode (int nodeId) {
4323 version(aabbtree_many_asserts) assert(nodeId >= 0 && nodeId < mAllocCount);
4324 version(aabbtree_many_asserts) assert(mNodes[nodeId].leaf);
4326 // if we are removing the root node (root node is a leaf in this case)
4327 if (mRootNodeId == nodeId) { mRootNodeId = TreeNode.NullTreeNode; return; }
4329 int parentNodeId = mNodes[nodeId].parentId;
4330 int grandParentNodeId = mNodes[parentNodeId].parentId;
4331 int siblingNodeId;
4333 if (mNodes[parentNodeId].children.ptr[TreeNode.Left] == nodeId) {
4334 siblingNodeId = mNodes[parentNodeId].children.ptr[TreeNode.Right];
4335 } else {
4336 siblingNodeId = mNodes[parentNodeId].children.ptr[TreeNode.Left];
4339 // if the parent of the node to remove is not the root node
4340 if (grandParentNodeId != TreeNode.NullTreeNode) {
4341 // destroy the parent node
4342 if (mNodes[grandParentNodeId].children.ptr[TreeNode.Left] == parentNodeId) {
4343 mNodes[grandParentNodeId].children.ptr[TreeNode.Left] = siblingNodeId;
4344 } else {
4345 version(aabbtree_many_asserts) assert(mNodes[grandParentNodeId].children.ptr[TreeNode.Right] == parentNodeId);
4346 mNodes[grandParentNodeId].children.ptr[TreeNode.Right] = siblingNodeId;
4348 mNodes[siblingNodeId].parentId = grandParentNodeId;
4349 releaseNode(parentNodeId);
4351 // now, we need to recompute the AABBs of the node on the path back to the root and make sure that the tree is still balanced
4352 int currentNodeId = grandParentNodeId;
4353 while (currentNodeId != TreeNode.NullTreeNode) {
4354 // balance the current sub-tree if necessary
4355 currentNodeId = balanceSubTreeAtNode(currentNodeId);
4357 version(aabbtree_many_asserts) assert(!mNodes[currentNodeId].leaf);
4359 // get the two children.ptr of the current node
4360 int leftChildId = mNodes[currentNodeId].children.ptr[TreeNode.Left];
4361 int rightChildId = mNodes[currentNodeId].children.ptr[TreeNode.Right];
4363 // recompute the AABB and the height of the current node
4364 mNodes[currentNodeId].aabb.merge(mNodes[leftChildId].aabb, mNodes[rightChildId].aabb);
4365 mNodes[currentNodeId].height = cast(short)(max(mNodes[leftChildId].height, mNodes[rightChildId].height)+1);
4366 version(aabbtree_many_asserts) assert(mNodes[currentNodeId].height > 0);
4368 currentNodeId = mNodes[currentNodeId].parentId;
4370 } else {
4371 // if the parent of the node to remove is the root node, the sibling node becomes the new root node
4372 mRootNodeId = siblingNodeId;
4373 mNodes[siblingNodeId].parentId = TreeNode.NullTreeNode;
4374 releaseNode(parentNodeId);
4378 // balance the sub-tree of a given node using left or right rotations
4379 // the rotation schemes are described in the book "Introduction to Game Physics with Box2D" by Ian Parberry
4380 // this method returns the new root node id
4381 int balanceSubTreeAtNode (int nodeId) {
4382 version(aabbtree_many_asserts) assert(nodeId != TreeNode.NullTreeNode);
4384 TreeNode* nodeA = mNodes+nodeId;
4386 // if the node is a leaf or the height of A's sub-tree is less than 2
4387 if (nodeA.leaf || nodeA.height < 2) return nodeId; // do not perform any rotation
4389 // get the two children nodes
4390 int nodeBId = nodeA.children.ptr[TreeNode.Left];
4391 int nodeCId = nodeA.children.ptr[TreeNode.Right];
4392 version(aabbtree_many_asserts) assert(nodeBId >= 0 && nodeBId < mAllocCount);
4393 version(aabbtree_many_asserts) assert(nodeCId >= 0 && nodeCId < mAllocCount);
4394 TreeNode* nodeB = mNodes+nodeBId;
4395 TreeNode* nodeC = mNodes+nodeCId;
4397 // compute the factor of the left and right sub-trees
4398 int balanceFactor = nodeC.height-nodeB.height;
4400 // if the right node C is 2 higher than left node B
4401 if (balanceFactor > 1) {
4402 version(aabbtree_many_asserts) assert(!nodeC.leaf);
4404 int nodeFId = nodeC.children.ptr[TreeNode.Left];
4405 int nodeGId = nodeC.children.ptr[TreeNode.Right];
4406 version(aabbtree_many_asserts) assert(nodeFId >= 0 && nodeFId < mAllocCount);
4407 version(aabbtree_many_asserts) assert(nodeGId >= 0 && nodeGId < mAllocCount);
4408 TreeNode* nodeF = mNodes+nodeFId;
4409 TreeNode* nodeG = mNodes+nodeGId;
4411 nodeC.children.ptr[TreeNode.Left] = nodeId;
4412 nodeC.parentId = nodeA.parentId;
4413 nodeA.parentId = nodeCId;
4415 if (nodeC.parentId != TreeNode.NullTreeNode) {
4416 if (mNodes[nodeC.parentId].children.ptr[TreeNode.Left] == nodeId) {
4417 mNodes[nodeC.parentId].children.ptr[TreeNode.Left] = nodeCId;
4418 } else {
4419 version(aabbtree_many_asserts) assert(mNodes[nodeC.parentId].children.ptr[TreeNode.Right] == nodeId);
4420 mNodes[nodeC.parentId].children.ptr[TreeNode.Right] = nodeCId;
4422 } else {
4423 mRootNodeId = nodeCId;
4426 version(aabbtree_many_asserts) assert(!nodeC.leaf);
4427 version(aabbtree_many_asserts) assert(!nodeA.leaf);
4429 // if the right node C was higher than left node B because of the F node
4430 if (nodeF.height > nodeG.height) {
4431 nodeC.children.ptr[TreeNode.Right] = nodeFId;
4432 nodeA.children.ptr[TreeNode.Right] = nodeGId;
4433 nodeG.parentId = nodeId;
4435 // recompute the AABB of node A and C
4436 nodeA.aabb.merge(nodeB.aabb, nodeG.aabb);
4437 nodeC.aabb.merge(nodeA.aabb, nodeF.aabb);
4439 // recompute the height of node A and C
4440 nodeA.height = cast(short)(max(nodeB.height, nodeG.height)+1);
4441 nodeC.height = cast(short)(max(nodeA.height, nodeF.height)+1);
4442 version(aabbtree_many_asserts) assert(nodeA.height > 0);
4443 version(aabbtree_many_asserts) assert(nodeC.height > 0);
4444 } else {
4445 // if the right node C was higher than left node B because of node G
4446 nodeC.children.ptr[TreeNode.Right] = nodeGId;
4447 nodeA.children.ptr[TreeNode.Right] = nodeFId;
4448 nodeF.parentId = nodeId;
4450 // recompute the AABB of node A and C
4451 nodeA.aabb.merge(nodeB.aabb, nodeF.aabb);
4452 nodeC.aabb.merge(nodeA.aabb, nodeG.aabb);
4454 // recompute the height of node A and C
4455 nodeA.height = cast(short)(max(nodeB.height, nodeF.height)+1);
4456 nodeC.height = cast(short)(max(nodeA.height, nodeG.height)+1);
4457 version(aabbtree_many_asserts) assert(nodeA.height > 0);
4458 version(aabbtree_many_asserts) assert(nodeC.height > 0);
4461 // return the new root of the sub-tree
4462 return nodeCId;
4465 // if the left node B is 2 higher than right node C
4466 if (balanceFactor < -1) {
4467 version(aabbtree_many_asserts) assert(!nodeB.leaf);
4469 int nodeFId = nodeB.children.ptr[TreeNode.Left];
4470 int nodeGId = nodeB.children.ptr[TreeNode.Right];
4471 version(aabbtree_many_asserts) assert(nodeFId >= 0 && nodeFId < mAllocCount);
4472 version(aabbtree_many_asserts) assert(nodeGId >= 0 && nodeGId < mAllocCount);
4473 TreeNode* nodeF = mNodes+nodeFId;
4474 TreeNode* nodeG = mNodes+nodeGId;
4476 nodeB.children.ptr[TreeNode.Left] = nodeId;
4477 nodeB.parentId = nodeA.parentId;
4478 nodeA.parentId = nodeBId;
4480 if (nodeB.parentId != TreeNode.NullTreeNode) {
4481 if (mNodes[nodeB.parentId].children.ptr[TreeNode.Left] == nodeId) {
4482 mNodes[nodeB.parentId].children.ptr[TreeNode.Left] = nodeBId;
4483 } else {
4484 version(aabbtree_many_asserts) assert(mNodes[nodeB.parentId].children.ptr[TreeNode.Right] == nodeId);
4485 mNodes[nodeB.parentId].children.ptr[TreeNode.Right] = nodeBId;
4487 } else {
4488 mRootNodeId = nodeBId;
4491 version(aabbtree_many_asserts) assert(!nodeB.leaf);
4492 version(aabbtree_many_asserts) assert(!nodeA.leaf);
4494 // if the left node B was higher than right node C because of the F node
4495 if (nodeF.height > nodeG.height) {
4496 nodeB.children.ptr[TreeNode.Right] = nodeFId;
4497 nodeA.children.ptr[TreeNode.Left] = nodeGId;
4498 nodeG.parentId = nodeId;
4500 // recompute the AABB of node A and B
4501 nodeA.aabb.merge(nodeC.aabb, nodeG.aabb);
4502 nodeB.aabb.merge(nodeA.aabb, nodeF.aabb);
4504 // recompute the height of node A and B
4505 nodeA.height = cast(short)(max(nodeC.height, nodeG.height)+1);
4506 nodeB.height = cast(short)(max(nodeA.height, nodeF.height)+1);
4507 version(aabbtree_many_asserts) assert(nodeA.height > 0);
4508 version(aabbtree_many_asserts) assert(nodeB.height > 0);
4509 } else {
4510 // if the left node B was higher than right node C because of node G
4511 nodeB.children.ptr[TreeNode.Right] = nodeGId;
4512 nodeA.children.ptr[TreeNode.Left] = nodeFId;
4513 nodeF.parentId = nodeId;
4515 // recompute the AABB of node A and B
4516 nodeA.aabb.merge(nodeC.aabb, nodeF.aabb);
4517 nodeB.aabb.merge(nodeA.aabb, nodeG.aabb);
4519 // recompute the height of node A and B
4520 nodeA.height = cast(short)(max(nodeC.height, nodeF.height)+1);
4521 nodeB.height = cast(short)(max(nodeA.height, nodeG.height)+1);
4522 version(aabbtree_many_asserts) assert(nodeA.height > 0);
4523 version(aabbtree_many_asserts) assert(nodeB.height > 0);
4526 // return the new root of the sub-tree
4527 return nodeBId;
4530 // if the sub-tree is balanced, return the current root node
4531 return nodeId;
4534 // compute the height of a given node in the tree
4535 int computeHeight (int nodeId) {
4536 version(aabbtree_many_asserts) assert(nodeId >= 0 && nodeId < mAllocCount);
4537 TreeNode* node = mNodes+nodeId;
4539 // if the node is a leaf, its height is zero
4540 if (node.leaf) return 0;
4542 // compute the height of the left and right sub-tree
4543 int leftHeight = computeHeight(node.children.ptr[TreeNode.Left]);
4544 int rightHeight = computeHeight(node.children.ptr[TreeNode.Right]);
4546 // return the height of the node
4547 return 1+max(leftHeight, rightHeight);
4550 // internally add an object into the tree
4551 int insertObjectInternal (in ref AABB aabb, bool staticObject) {
4552 // get the next available node (or allocate new ones if necessary)
4553 int nodeId = allocateNode();
4555 // create the fat aabb to use in the tree
4556 mNodes[nodeId].aabb = aabb;
4557 if (!staticObject) {
4558 static if (VT.Dims == 2) {
4559 immutable gap = VT(mExtraGap, mExtraGap);
4560 } else {
4561 immutable gap = VT(mExtraGap, mExtraGap, mExtraGap);
4563 mNodes[nodeId].aabb.min -= gap;
4564 mNodes[nodeId].aabb.max += gap;
4567 // set the height of the node in the tree
4568 mNodes[nodeId].height = 0;
4570 // insert the new leaf node in the tree
4571 insertLeafNode(nodeId);
4572 version(aabbtree_many_asserts) assert(mNodes[nodeId].leaf);
4574 version(aabbtree_many_asserts) assert(nodeId >= 0);
4576 // return the id of the node
4577 return nodeId;
4580 // initialize the tree
4581 void setup () {
4582 import core.stdc.stdlib : malloc;
4583 import core.stdc.string : memset;
4585 mRootNodeId = TreeNode.NullTreeNode;
4586 mNodeCount = 0;
4587 mAllocCount = 64;
4589 mNodes = cast(TreeNode*)malloc(mAllocCount*TreeNode.sizeof);
4590 if (mNodes is null) assert(0, "out of memory");
4591 memset(mNodes, 0, mAllocCount*TreeNode.sizeof);
4593 // initialize the allocated nodes
4594 foreach (int i; 0..mAllocCount-1) {
4595 mNodes[i].nextNodeId = i+1;
4596 mNodes[i].height = -1;
4598 mNodes[mAllocCount-1].nextNodeId = TreeNode.NullTreeNode;
4599 mNodes[mAllocCount-1].height = -1;
4600 mFreeNodeId = 0;
4603 // also, checks if the tree structure is valid (for debugging purpose)
4604 public void forEachLeaf (scope void delegate (BodyBase abody, in ref AABB aabb) dg) {
4605 void forEachNode (int nodeId) {
4606 if (nodeId == TreeNode.NullTreeNode) return;
4607 // if it is the root
4608 if (nodeId == mRootNodeId) {
4609 assert(mNodes[nodeId].parentId == TreeNode.NullTreeNode);
4611 // get the children nodes
4612 TreeNode* pNode = mNodes+nodeId;
4613 assert(pNode.height >= 0);
4614 assert(pNode.aabb.volume > 0);
4615 // if the current node is a leaf
4616 if (pNode.leaf) {
4617 assert(pNode.height == 0);
4618 if (dg !is null) dg(pNode.flesh, pNode.aabb);
4619 } else {
4620 int leftChild = pNode.children.ptr[TreeNode.Left];
4621 int rightChild = pNode.children.ptr[TreeNode.Right];
4622 // check that the children node Ids are valid
4623 assert(0 <= leftChild && leftChild < mAllocCount);
4624 assert(0 <= rightChild && rightChild < mAllocCount);
4625 // check that the children nodes have the correct parent node
4626 assert(mNodes[leftChild].parentId == nodeId);
4627 assert(mNodes[rightChild].parentId == nodeId);
4628 // check the height of node
4629 int height = 1+max(mNodes[leftChild].height, mNodes[rightChild].height);
4630 assert(mNodes[nodeId].height == height);
4631 // check the AABB of the node
4632 AABB aabb = AABB.mergeAABBs(mNodes[leftChild].aabb, mNodes[rightChild].aabb);
4633 assert(aabb.min == mNodes[nodeId].aabb.min);
4634 assert(aabb.max == mNodes[nodeId].aabb.max);
4635 // recursively check the children nodes
4636 forEachNode(leftChild);
4637 forEachNode(rightChild);
4640 // recursively check each node
4641 forEachNode(mRootNodeId);
4644 static if (GCAnchor) void gcRelease () {
4645 import core.memory : GC;
4646 foreach (ref TreeNode n; mNodes[0..mNodeCount]) {
4647 if (n.leaf) {
4648 auto flesh = n.flesh;
4649 GC.clrAttr(*cast(void**)&flesh, GC.BlkAttr.NO_MOVE);
4650 GC.removeRoot(*cast(void**)&flesh);
4655 version(aabbtree_query_count) public int nodesVisited, nodesDeepVisited;
4657 // return `true` from visitor to stop immediately
4658 // checker should check if this node should be considered to further checking
4659 // returns tree node if visitor says stop or -1
4660 private int visit (scope bool delegate (TreeNode* node) checker, scope bool delegate (BodyBase abody) visitor) {
4661 int[1024] stack = void; // stack with the nodes to visit
4662 int sp = 0;
4663 int[] bigstack = null;
4664 scope(exit) if (bigstack.ptr !is null) delete bigstack;
4666 void spush (int id) {
4667 if (sp < stack.length) {
4668 // use "small stack"
4669 stack.ptr[sp++] = id;
4670 } else {
4671 if (sp >= int.max/2) assert(0, "huge tree!");
4672 // use "big stack"
4673 immutable int xsp = sp-cast(int)stack.length;
4674 if (xsp < bigstack.length) {
4675 // reuse
4676 bigstack.ptr[xsp] = id;
4677 } else {
4678 // grow
4679 auto optr = bigstack.ptr;
4680 bigstack ~= id;
4681 if (bigstack.ptr !is optr) {
4682 import core.memory : GC;
4683 optr = bigstack.ptr;
4684 if (optr is GC.addrOf(optr)) GC.setAttr(optr, GC.BlkAttr.NO_INTERIOR);
4687 ++sp;
4691 int spop () {
4692 pragma(inline, true); // why not?
4693 if (sp == 0) assert(0, "stack underflow");
4694 if (sp <= stack.length) {
4695 // use "small stack"
4696 return stack.ptr[--sp];
4697 } else {
4698 // use "big stack"
4699 --sp;
4700 return bigstack.ptr[sp-cast(int)stack.length];
4704 version(aabbtree_query_count) nodesVisited = nodesDeepVisited = 0;
4706 // start from root node
4707 spush(mRootNodeId);
4709 // while there are still nodes to visit
4710 while (sp > 0) {
4711 // get the next node id to visit
4712 int nodeId = spop();
4713 // skip it if it is a null node
4714 if (nodeId == TreeNode.NullTreeNode) continue;
4715 version(aabbtree_query_count) ++nodesVisited;
4716 // get the corresponding node
4717 TreeNode* node = mNodes+nodeId;
4718 // should we investigate this node?
4719 if (checker(node)) {
4720 // if the node is a leaf
4721 if (node.leaf) {
4722 // call visitor on it
4723 version(aabbtree_query_count) ++nodesDeepVisited;
4724 if (visitor(node.flesh)) return nodeId;
4725 } else {
4726 // if the node is not a leaf, we need to visit its children
4727 spush(node.children.ptr[TreeNode.Left]);
4728 spush(node.children.ptr[TreeNode.Right]);
4733 return -1; // oops
4736 public:
4737 /// add `extraAABBGap` to bounding boxes so slight object movement won't cause tree rebuilds
4738 /// extra AABB Gap used to allow the collision shape to move a little bit without triggering a large modification of the tree which can be costly
4739 this (Float extraAABBGap=FloatNum!0) nothrow {
4740 mExtraGap = extraAABBGap;
4741 setup();
4744 ~this () {
4745 import core.stdc.stdlib : free;
4746 static if (GCAnchor) gcRelease();
4747 free(mNodes);
4750 /// return the root AABB of the tree
4751 AABB getRootAABB () nothrow @trusted @nogc {
4752 pragma(inline, true);
4753 version(aabbtree_many_asserts) assert(mRootNodeId >= 0 && mRootNodeId < mAllocCount);
4754 return mNodes[mRootNodeId].aabb;
4757 /// does the given id represents a valid object?
4758 /// WARNING: ids of removed objects can be reused on later insertions!
4759 bool isValidId (int id) const nothrow @trusted @nogc { pragma(inline, true); return (id >= 0 && id < mAllocCount && mNodes[id].leaf); }
4761 /// get current extra AABB gap
4762 @property Float extraGap () const pure nothrow @trusted @nogc { pragma(inline, true); return mExtraGap; }
4764 /// set current extra AABB gap
4765 @property void extraGap (Float aExtraGap) pure nothrow @trusted @nogc { pragma(inline, true); mExtraGap = aExtraGap; }
4767 /// get object by id; can return null for invalid ids
4768 BodyBase getObject (int id) nothrow @trusted @nogc { pragma(inline, true); return (id >= 0 && id < mAllocCount && mNodes[id].leaf ? mNodes[id].flesh : null); }
4770 /// get fat object AABB by id; returns random shit for invalid ids
4771 AABB getObjectFatAABB (int id) nothrow @trusted @nogc { pragma(inline, true); return (id >= 0 && id < mAllocCount && !mNodes[id].free ? mNodes[id].aabb : AABB()); }
4773 /// insert an object into the tree
4774 /// this method creates a new leaf node in the tree and returns the id of the corresponding node
4775 /// AABB for static object will not be "fat" (simple optimization)
4776 /// WARNING! inserting the same object several times *WILL* break everything!
4777 int insertObject (BodyBase flesh, bool staticObject=false) {
4778 auto aabb = flesh.getAABB(); // can be passed as argument
4779 int nodeId = insertObjectInternal(aabb, staticObject);
4780 version(aabbtree_many_asserts) assert(mNodes[nodeId].leaf);
4781 mNodes[nodeId].flesh = flesh;
4782 static if (GCAnchor) {
4783 import core.memory : GC;
4784 GC.addRoot(*cast(void**)&flesh);
4785 GC.setAttr(*cast(void**)&flesh, GC.BlkAttr.NO_MOVE);
4787 return nodeId;
4790 /// remove an object from the tree
4791 /// WARNING: ids of removed objects can be reused on later insertions!
4792 void removeObject (int nodeId) {
4793 if (nodeId < 0 || nodeId >= mAllocCount || !mNodes[nodeId].leaf) assert(0, "invalid node id");
4794 static if (GCAnchor) {
4795 import core.memory : GC;
4796 auto flesh = mNodes[nodeId].flesh;
4797 GC.clrAttr(*cast(void**)&flesh, GC.BlkAttr.NO_MOVE);
4798 GC.removeRoot(*cast(void**)&flesh);
4800 // remove the node from the tree
4801 removeLeafNode(nodeId);
4802 releaseNode(nodeId);
4805 /** update the dynamic tree after an object has moved.
4807 * if the new AABB of the object that has moved is still inside its fat AABB, then nothing is done.
4808 * otherwise, the corresponding node is removed and reinserted into the tree.
4809 * the method returns true if the object has been reinserted into the tree.
4810 * the "displacement" parameter is the linear velocity of the AABB multiplied by the elapsed time between two frames.
4811 * if the "forceReinsert" parameter is true, we force a removal and reinsertion of the node
4812 * (this can be useful if the shape AABB has become much smaller than the previous one for instance).
4814 * note that you should call this method if body's AABB was modified, even if the body wasn't moved.
4816 * if `forceReinsert` == `true` and `displacement` is zero, convert object to "static" (don't extrude AABB).
4818 * return `true` if the tree was modified.
4820 bool updateObject() (int nodeId, in auto ref AABB.VType displacement, bool forceReinsert=false) {
4821 if (nodeId < 0 || nodeId >= mAllocCount || !mNodes[nodeId].leaf) assert(0, "invalid node id");
4823 auto newAABB = mNodes[nodeId].flesh.getAABB(); // can be passed as argument
4825 // if the new AABB is still inside the fat AABB of the node
4826 if (!forceReinsert && mNodes[nodeId].aabb.contains(newAABB)) return false;
4828 // if the new AABB is outside the fat AABB, we remove the corresponding node
4829 removeLeafNode(nodeId);
4831 // compute the fat AABB by inflating the AABB with a constant gap
4832 mNodes[nodeId].aabb = newAABB;
4833 if (!(forceReinsert && displacement.isZero)) {
4834 static if (VT.Dims == 2) {
4835 immutable gap = VT(mExtraGap, mExtraGap);
4836 } else {
4837 immutable gap = VT(mExtraGap, mExtraGap, mExtraGap);
4839 mNodes[nodeId].aabb.mMin -= gap;
4840 mNodes[nodeId].aabb.mMax += gap;
4843 // inflate the fat AABB in direction of the linear motion of the AABB
4844 if (displacement.x < FloatNum!0) {
4845 mNodes[nodeId].aabb.mMin.x += LinearMotionGapMultiplier*displacement.x;
4846 } else {
4847 mNodes[nodeId].aabb.mMax.x += LinearMotionGapMultiplier*displacement.x;
4849 if (displacement.y < FloatNum!0) {
4850 mNodes[nodeId].aabb.mMin.y += LinearMotionGapMultiplier*displacement.y;
4851 } else {
4852 mNodes[nodeId].aabb.mMax.y += LinearMotionGapMultiplier*displacement.y;
4854 static if (AABB.VType.Dims == 3) {
4855 if (displacement.z < FloatNum!0) {
4856 mNodes[nodeId].aabb.mMin.z += LinearMotionGapMultiplier*displacement.z;
4857 } else {
4858 mNodes[nodeId].aabb.mMax.z += LinearMotionGapMultiplier*displacement.z;
4862 version(aabbtree_many_asserts) assert(mNodes[nodeId].aabb.contains(newAABB));
4864 // reinsert the node into the tree
4865 insertLeafNode(nodeId);
4867 return true;
4870 /// report all shapes overlapping with the AABB given in parameter
4871 void forEachAABBOverlap() (in auto ref AABB aabb, scope OverlapCallback cb) {
4872 visit(
4873 // checker
4874 (node) => aabb.overlaps(node.aabb),
4875 // visitor
4876 (flesh) { cb(flesh); return false; }
4880 /// report body that contains the given point
4881 BodyBase pointQuery() (in auto ref VT point, scope SimpleQueryCallback cb) {
4882 int nid = visit(
4883 // checker
4884 (node) => node.aabb.contains(point),
4885 // visitor
4886 (flesh) => cb(flesh),
4888 version(aabbtree_many_asserts) assert(nid < 0 || (nid >= 0 && nid < mAllocCount && mNodes[nid].leaf));
4889 return (nid >= 0 ? mNodes[nid].flesh : null);
4892 /// report all bodies containing the given point
4893 void pointQuery() (in auto ref VT point, scope SimpleQueryCallbackNR cb) {
4894 visit(
4895 // checker
4896 (node) => node.aabb.contains(point),
4897 // visitor
4898 (flesh) { cb(flesh); return false; },
4903 static struct SegmentQueryResult {
4904 Float dist = -1; /// <0: nothing was hit
4905 BodyBase flesh; ///
4907 @property bool valid () const nothrow @safe @nogc { pragma(inline, true); return (dist >= 0 && flesh !is null); } ///
4910 /// segment querying method
4911 SegmentQueryResult segmentQuery() (in auto ref VT a, in auto ref VT b, scope SegQueryCallback cb) {
4912 SegmentQueryResult res;
4913 Float maxFraction = Float.infinity;
4915 immutable VT cura = a;
4916 VT curb = b;
4917 immutable VT dir = (curb-cura).normalized;
4919 visit(
4920 // checker
4921 (node) => node.aabb.isIntersects(cura, curb),
4922 // visitor
4923 (flesh) {
4924 Float hitFraction = cb(flesh, cura, curb);
4925 // if the user returned a hitFraction of zero, it means that the raycasting should stop here
4926 if (hitFraction == FloatNum!0) {
4927 res.dist = 0;
4928 res.flesh = flesh;
4929 return true;
4931 // if the user returned a positive fraction
4932 if (hitFraction > FloatNum!0) {
4933 // we update the maxFraction value and the ray AABB using the new maximum fraction
4934 if (hitFraction < maxFraction) {
4935 maxFraction = hitFraction;
4936 res.dist = hitFraction;
4937 res.flesh = flesh;
4938 // fix curb here
4939 curb = cura+dir*hitFraction;
4942 return false; // continue
4946 return res;
4949 /// compute the height of the tree
4950 int computeHeight () { pragma(inline, true); return computeHeight(mRootNodeId); }
4952 @property int nodeCount () const pure nothrow @safe @nogc { pragma(inline, true); return mNodeCount; }
4953 @property int nodeAlloced () const pure nothrow @safe @nogc { pragma(inline, true); return mAllocCount; }
4955 /// clear all the nodes and reset the tree
4956 void reset() {
4957 import core.stdc.stdlib : free;
4958 static if (GCAnchor) gcRelease();
4959 free(mNodes);
4960 setup();
4966 // ////////////////////////////////////////////////////////////////////////// //
4967 final class Body {
4968 AABB aabb;
4970 this (vec2 amin, vec2 amax) { aabb.min = amin; aabb.max = amax; }
4972 AABB getAABB () { return aabb; }
4976 // ////////////////////////////////////////////////////////////////////////// //
4977 import iv.vfs.io;
4979 void main () {
4980 auto tree = new DynamicAABBTree!Body(0.2);
4982 vec2 bmin = vec2(10, 15);
4983 vec2 bmax = vec2(42, 54);
4985 auto flesh = new Body(bmin, bmax);
4987 tree.insertObject(flesh);
4989 vec2 ro = vec2(5, 18);
4990 vec2 rd = vec2(1, 0.2).normalized;
4991 vec2 re = ro+rd*20;
4993 writeln(flesh.aabb.segIntersectMin(ro, re));
4995 auto res = tree.segmentQuery(ro, re, delegate (flesh, in ref vec2 a, in ref vec2 b) {
4996 auto dst = flesh.aabb.segIntersectMin(a, b);
4997 writeln("a=", a, "; b=", b, "; dst=", dst);
4998 if (dst < 0) return -1;
4999 return dst;
5002 writeln(res);