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, either version 3 of the License, or
8 * (at your option) any later version.
10 * This program is distributed in the hope that it will be useful,
11 * but WITHOUT ANY WARRANTY; without even the implied warranty of
12 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 * GNU General Public License for more details.
15 * You should have received a copy of the GNU General Public License
16 * along with this program. If not, see <http://www.gnu.org/licenses/>.
18 // various vector and matrix operations
19 // matrix should be compatible with OpenGL, but mostly untested
20 module iv
.vmath
/*is aliced*/;
23 //version = aabbtree_many_asserts;
24 version = aabbtree_query_count
;
25 version = vmath_slow_normalize
;
28 // ////////////////////////////////////////////////////////////////////////// //
29 version(vmath_double
) {
30 alias VFloat
= double;
34 enum VFloatNum(long v
) = cast(VFloat
)v
;
35 enum VFloatNum(float v
) = cast(VFloat
)v
;
36 enum VFloatNum(double v
) = cast(VFloat
)v
;
37 enum VFloatNum(real v
) = cast(VFloat
)v
;
40 // ////////////////////////////////////////////////////////////////////////// //
41 /// is `v` finite (i.e. not a nan and not an infinity)?
42 public bool isFiniteDbl (in double v
) pure nothrow @trusted @nogc {
44 return (((*cast(const(ulong)*)&v
)&0x7ff0_0000_0000_0000UL) != 0x7ff0_0000_0000_0000UL);
48 /// is `v` finite (i.e. not a nan and not an infinity)?
49 public bool isFiniteFlt (in float v
) pure nothrow @trusted @nogc {
51 return (((*cast(const(uint)*)&v
)&0x7f80_0000U) != 0x7f80_0000U);
55 // ////////////////////////////////////////////////////////////////////////// //
56 private template isGoodSwizzling(string s
, string comps
, int minlen
, int maxlen
) {
57 private static template hasChar(string
str, char ch
, uint idx
=0) {
58 static if (idx
>= str.length
) enum hasChar
= false;
59 else static if (str[idx
] == ch
) enum hasChar
= true;
60 else enum hasChar
= hasChar
!(str, ch
, idx
+1);
62 private static template swz(string
str, string comps
, uint idx
=0) {
63 static if (idx
>= str.length
) enum swz
= true;
64 else static if (hasChar
!(comps
, str[idx
])) enum swz
= swz
!(str, comps
, idx
+1);
65 else enum swz
= false;
67 static if (s
.length
>= minlen
&& s
.length
<= maxlen
) enum isGoodSwizzling
= swz
!(s
, comps
);
68 else enum isGoodSwizzling
= false;
71 private template SwizzleCtor(string stn
, string s
) {
72 private static template buildCtor (string s
, uint idx
) {
73 static if (idx
>= s
.length
) enum buildCtor
= "";
74 else enum buildCtor
= s
[idx
]~","~buildCtor
!(s
, idx
+1);
76 enum SwizzleCtor
= stn
~"("~buildCtor
!(s
, 0)~")";
80 // ////////////////////////////////////////////////////////////////////////// //
81 // Values obtained from Wolfram Alpha. 116 bits ought to be enough for anybody.
82 // Wolfram Alpha LLC. 2011. Wolfram|Alpha. http://www.wolframalpha.com/input/?i=e+in+base+16 (access July 6, 2011).
83 enum real E_R
= 0x1.5bf0a
8b1457695355fb8ac
404e
7a
8p
+1L; /* e = 2.718281... */
84 enum real LOG2T_R
= 0x1.a934f0979a3715fc9257edfe9b5fbp
+1L; /* log2 10 = 3.321928... */
85 enum real LOG2E_R
= 0x1.71547652b82fe
1777d0ffda
0d23a
8p
+0L; /* log2 e = 1.442695... */
86 enum real LOG2_R
= 0x1.34413509f79fef
311f12b35816f92p
-2L; /* log10 2 = 0.301029... */
87 enum real LOG10E_R
= 0x1.bcb7b1526e50e32a6ab7555f5a67cp
-2L; /* log10 e = 0.434294... */
88 enum real LN2_R
= 0x1.62e42fefa39ef35793c7673007e5fp
-1L; /* ln 2 = 0.693147... */
89 enum real LN10_R
= 0x1.26bb1bbb5551582dd4adac
5705a
61p
+1L; /* ln 10 = 2.302585... */
90 enum real PI_R
= 0x1.921fb54442d18469898cc
51701b84p
+1L; /* PI = 3.141592... */
91 enum real PI_2_R
= PI_R
/2; /* PI / 2 = 1.570796... */
92 enum real PI_4_R
= PI_R
/4; /* PI / 4 = 0.785398... */
93 enum real M_1_PI_R
= 0x1.45f306dc
9c
882a
53f84eafa
3ea
69cp
-2L; /* 1 / PI = 0.318309... */
94 enum real M_2_PI_R
= 2*M_1_PI_R
; /* 2 / PI = 0.636619... */
95 enum real M_2_SQRTPI_R
= 0x1.20dd750429b6d11ae
3a
914fed
7fd8p
+0L; /* 2 / sqrt(PI) = 1.128379... */
96 enum real SQRT2_R
= 0x1.6a09e667f3bcc908b2fb1366ea958p
+0L; /* sqrt(2) = 1.414213... */
97 enum real SQRT1_2_R
= SQRT2_R
/2; /* sqrt(1/2) = 0.707106... */
99 enum double E_D
= cast(double)0x1.5bf0a
8b1457695355fb8ac
404e
7a
8p
+1L; /* e = 2.718281... */
100 enum double LOG2T_D
= cast(double)0x1.a934f0979a3715fc9257edfe9b5fbp
+1L; /* log2 10 = 3.321928... */
101 enum double LOG2E_D
= cast(double)0x1.71547652b82fe
1777d0ffda
0d23a
8p
+0L; /* log2 e = 1.442695... */
102 enum double LOG2_D
= cast(double)0x1.34413509f79fef
311f12b35816f92p
-2L; /* log10 2 = 0.301029... */
103 enum double LOG10E_D
= cast(double)0x1.bcb7b1526e50e32a6ab7555f5a67cp
-2L; /* log10 e = 0.434294... */
104 enum double LN2_D
= cast(double)0x1.62e42fefa39ef35793c7673007e5fp
-1L; /* ln 2 = 0.693147... */
105 enum double LN10_D
= cast(double)0x1.26bb1bbb5551582dd4adac
5705a
61p
+1L; /* ln 10 = 2.302585... */
106 enum double PI_D
= cast(double)0x1.921fb54442d18469898cc
51701b84p
+1L; /* PI = 3.141592... */
107 enum double PI_2_D
= cast(double)(PI_R
/2); /* PI / 2 = 1.570796... */
108 enum double PI_4_D
= cast(double)(PI_R
/4); /* PI / 4 = 0.785398... */
109 enum double M_1_PI_D
= cast(double)0x1.45f306dc
9c
882a
53f84eafa
3ea
69cp
-2L; /* 1 / PI = 0.318309... */
110 enum double M_2_PI_D
= cast(double)(2*M_1_PI_R
); /* 2 / PI = 0.636619... */
111 enum double M_2_SQRTPI_D
= cast(double)0x1.20dd750429b6d11ae
3a
914fed
7fd8p
+0L; /* 2 / sqrt(PI) = 1.128379... */
112 enum double SQRT2_D
= cast(double)0x1.6a09e667f3bcc908b2fb1366ea958p
+0L; /* sqrt(2) = 1.414213... */
113 enum double SQRT1_2_D
= cast(double)(SQRT2_R
/2); /* sqrt(1/2) = 0.707106... */
115 enum float E_F
= cast(float)0x1.5bf0a
8b1457695355fb8ac
404e
7a
8p
+1L; /* e = 2.718281... */
116 enum float LOG2T_F
= cast(float)0x1.a934f0979a3715fc9257edfe9b5fbp
+1L; /* log2 10 = 3.321928... */
117 enum float LOG2E_F
= cast(float)0x1.71547652b82fe
1777d0ffda
0d23a
8p
+0L; /* log2 e = 1.442695... */
118 enum float LOG2_F
= cast(float)0x1.34413509f79fef
311f12b35816f92p
-2L; /* log10 2 = 0.301029... */
119 enum float LOG10E_F
= cast(float)0x1.bcb7b1526e50e32a6ab7555f5a67cp
-2L; /* log10 e = 0.434294... */
120 enum float LN2_F
= cast(float)0x1.62e42fefa39ef35793c7673007e5fp
-1L; /* ln 2 = 0.693147... */
121 enum float LN10_F
= cast(float)0x1.26bb1bbb5551582dd4adac
5705a
61p
+1L; /* ln 10 = 2.302585... */
122 enum float PI_F
= cast(float)0x1.921fb54442d18469898cc
51701b84p
+1L; /* PI = 3.141592... */
123 enum float PI_2_F
= cast(float)(PI_R
/2); /* PI / 2 = 1.570796... */
124 enum float PI_4_F
= cast(float)(PI_R
/4); /* PI / 4 = 0.785398... */
125 enum float M_1_PI_F
= cast(float)0x1.45f306dc
9c
882a
53f84eafa
3ea
69cp
-2L; /* 1 / PI = 0.318309... */
126 enum float M_2_PI_F
= cast(float)(2*M_1_PI_R
); /* 2 / PI = 0.636619... */
127 enum float M_2_SQRTPI_F
= cast(float)0x1.20dd750429b6d11ae
3a
914fed
7fd8p
+0L; /* 2 / sqrt(PI) = 1.128379... */
128 enum float SQRT2_F
= cast(float)0x1.6a09e667f3bcc908b2fb1366ea958p
+0L; /* sqrt(2) = 1.414213... */
129 enum float SQRT1_2_F
= cast(float)(SQRT2_R
/2); /* sqrt(1/2) = 0.707106... */
132 // ////////////////////////////////////////////////////////////////////////// //
133 private template IsKnownVMathConstant(string name
) {
134 static if (name
== "E" || name
== "LOG2T" || name
== "LOG2E" || name
== "LOG2" || name
== "LOG10E" ||
135 name
== "LN2" || name
== "LN10" || name
== "PI" || name
== "PI_2" || name
== "PI_4" ||
136 name
== "M_1_PI" || name
== "M_2_PI" || name
== "M_2_SQRTPI" || name
== "SQRT2" || name
== "SQRT1_2")
138 enum IsKnownVMathConstant
= true;
140 enum IsKnownVMathConstant
= false;
144 template ImportCoreMath(FloatType
, T
...) {
146 (is(FloatType
== float) ||
is(FloatType
== const float) ||
is(FloatType
== immutable float)) ||
147 (is(FloatType
== double) ||
is(FloatType
== const double) ||
is(FloatType
== immutable double)),
148 "import type should be `float` or `double`");
149 private template InternalImport(T
...) {
150 static if (T
.length
== 0) enum InternalImport
= "";
151 else static if (is(typeof(T
[0]) == string
)) {
152 static if (T
[0] == "fabs" || T
[0] == "cos" || T
[0] == "sin" || T
[0] == "sqrt") {
153 enum InternalImport
= "import core.math : "~T
[0]~";"~InternalImport
!(T
[1..$]);
154 } else static if (T
[0] == "min" || T
[0] == "nmin") {
155 enum InternalImport
= "static T "~T
[0]~"(T) (in T a, in T b) { pragma(inline, true); return (a < b ? a : b); }"~InternalImport
!(T
[1..$]);
156 } else static if (T
[0] == "max" || T
[0] == "nmax") {
157 enum InternalImport
= "static T "~T
[0]~"(T) (in T a, in T b) { pragma(inline, true); return (a > b ? a : b); }"~InternalImport
!(T
[1..$]);
158 } else static if (IsKnownVMathConstant
!(T
[0])) {
159 static if (is(FloatType
== float) ||
is(FloatType
== const float) ||
is(FloatType
== immutable float)) {
160 enum InternalImport
= "enum "~T
[0]~"="~T
[0]~"_F;"~InternalImport
!(T
[1..$]);
162 enum InternalImport
= "enum "~T
[0]~"="~T
[0]~"_D;"~InternalImport
!(T
[1..$]);
164 } else static if (T
[0] == "isnan") {
165 enum InternalImport
= "import core.stdc.math : isnan;"~InternalImport
!(T
[1..$]);
166 } else static if (T
[0] == "isfinite") {
167 static if (is(FloatType
== float) ||
is(FloatType
== const float) ||
is(FloatType
== immutable float)) {
168 enum InternalImport
= "import iv.nanpay : isfinite = isFinite;"~InternalImport
!(T
[1..$]);
170 enum InternalImport
= "import iv.nanpay : isfinite = isFiniteD;"~InternalImport
!(T
[1..$]);
172 } else static if (is(FloatType
== float) ||
is(FloatType
== const float) ||
is(FloatType
== immutable float)) {
173 enum InternalImport
= "import core.stdc.math : "~T
[0]~"="~T
[0]~"f;"~InternalImport
!(T
[1..$]);
175 enum InternalImport
= "import core.stdc.math : "~T
[0]~";"~InternalImport
!(T
[1..$]);
178 else static assert(0, "string expected");
180 static if (T
.length
> 0) {
181 enum ImportCoreMath
= InternalImport
!T
;
183 enum ImportCoreMath
= "{}";
188 // ////////////////////////////////////////////////////////////////////////// //
190 enum DBLEPS
= 1.0e-18;
191 template EPSILON(T
) if (is(T
== float) ||
is(T
== double)) {
192 static if (is(T
== float)) enum EPSILON
= FLTEPS
;
193 else static if (is(T
== double)) enum EPSILON
= DBLEPS
;
194 else static assert(0, "wtf?!");
196 template SMALLEPSILON(T
) if (is(T
== float) ||
is(T
== double)) {
197 static if (is(T
== float)) enum SMALLEPSILON
= 1e-5f;
198 else static if (is(T
== double)) enum SMALLEPSILON
= 1.0e-9;
199 else static assert(0, "wtf?!");
202 auto deg2rad(T
:double) (T v
) pure nothrow @safe @nogc {
203 pragma(inline
, true);
204 static if (__traits(isFloating
, T
)) {
205 static if (is(T
== float)) alias PI
= PI_F
; else alias PI
= PI_D
;
206 return cast(T
)(v
*cast(T
)PI
/cast(T
)180);
208 static if (is(VFloat
== float)) alias PI
= PI_F
; else alias PI
= PI_D
;
209 return cast(VFloat
)(cast(VFloat
)v
*cast(VFloat
)PI
/cast(VFloat
)180);
213 auto rad2deg(T
:double) (T v
) pure nothrow @safe @nogc {
214 pragma(inline
, true);
215 static if (__traits(isFloating
, T
)) {
216 static if (is(T
== float)) alias PI
= PI_F
; else alias PI
= PI_D
;
217 return cast(T
)(v
*cast(T
)180/cast(T
)PI
);
219 static if (is(VFloat
== float)) alias PI
= PI_F
; else alias PI
= PI_D
;
220 return cast(VFloat
)(cast(VFloat
)v
*cast(VFloat
)180/cast(VFloat
)PI
);
225 // ////////////////////////////////////////////////////////////////////////// //
228 //alias AABB2 = AABBImpl!vec2;
231 // ////////////////////////////////////////////////////////////////////////// //
232 template IsVector(VT
) {
233 static if (is(VT
== VecN
!(D
, T
), ubyte D
, T
)) {
234 enum IsVector
= (D
== 2 || D
== 3);
236 enum IsVector
= false;
240 template IsVectorDim(VT
, ubyte dim
) {
241 static if (is(VT
== VecN
!(D
, T
), ubyte D
, T
)) {
242 enum IsVectorDim
= (D
== dim
);
244 enum IsVectorDim
= false;
248 template IsVector(VT
, FT
) {
249 static if (is(VT
== VecN
!(D
, T
), ubyte D
, T
)) {
250 enum IsVector
= ((D
== 2 || D
== 3) && is(T
== FT
));
252 enum IsVector
= false;
256 template IsVectorDim(VT
, FT
, ubyte dim
) {
257 static if (is(VT
== VecN
!(D
, T
), ubyte D
, T
)) {
258 enum IsVectorDim
= (D
== dim
&& is(T
== FT
));
260 enum IsVectorDim
= false;
264 template VectorDims(VT
) {
265 static if (is(VT
== VecN
!(D
, F
), ubyte D
, F
)) {
272 template VectorFloat(VT
) {
273 static if (is(VT
== VecN
!(D
, F
), ubyte D
, F
)) {
274 alias VectorFloat
= F
;
276 static assert(0, "not a vector");
281 // ////////////////////////////////////////////////////////////////////////// //
282 align(1) struct VecN(ubyte dims
, FloatType
=VFloat
) if (dims
>= 2 && dims
<= 3 && (is(FloatType
== float) ||
is(FloatType
== double))) {
285 static T
nmin(T
) (in T a
, in T b
) { pragma(inline
, true); return (a
< b ? a
: b
); }
286 static T
nmax(T
) (in T a
, in T b
) { pragma(inline
, true); return (a
> b ? a
: b
); }
289 enum isVector(VT
) = (is(VT
== VecN
!(2, FloatType
)) ||
is(VT
== VecN
!(3, FloatType
)));
290 enum isVector2(VT
) = is(VT
== VecN
!(2, FloatType
));
291 enum isVector3(VT
) = is(VT
== VecN
!(3, FloatType
));
292 enum isSameVector(VT
) = is(VT
== VecN
!(dims
, FloatType
));
294 alias v2
= VecN
!(2, FloatType
);
295 alias v3
= VecN
!(3, FloatType
);
297 alias Me
= typeof(this);
298 alias Float
= FloatType
;
300 enum Epsilon
= EPSILON
!Float
;
305 static if (dims
>= 3) Float z
= 0;
308 string
toString () const {
309 import std
.string
: format
;
311 static if (dims
== 2) return "(%s,%s)".format(x
, y
);
312 else static if (dims
== 3) return "(%s,%s,%s)".format(x
, y
, z
);
313 else static assert(0, "invalid dimension count for vector");
314 } catch (Exception
) {
320 inout(Float
)* unsafePtr () inout nothrow @trusted @nogc { pragma(inline
, true); return cast(typeof(return))&x
; }
323 this (in Float
[] c
) @trusted {
324 x
= (c
.length
>= 1 ? c
.ptr
[0] : 0);
325 y
= (c
.length
>= 2 ? c
.ptr
[1] : 0);
326 static if (dims
== 3) z
= (c
.length
>= 3 ? c
.ptr
[2] : 0);
329 this (in Float ax
, in Float ay
) pure {
330 pragma(inline
, true);
333 static if (dims
== 3) z
= 0;
336 this (in Float ax
, in Float ay
, in Float az
) pure {
337 //pragma(inline, true);
340 static if (dims
== 3) z
= az
;
343 this(VT
) (in auto ref VT v
) pure if (isVector
!VT
) {
344 //pragma(inline, true);
347 static if (dims
== 3) {
348 static if (isVector3
!VT
) z
= v
.z
; else z
= 0;
352 static Me
Zero () pure nothrow @safe @nogc { pragma(inline
, true); return Me(0, 0); }
353 static Me
Invalid () pure nothrow @safe @nogc { pragma(inline
, true); return Me(Float
.nan
, Float
.nan
); }
355 // infinites are invalid too
356 @property bool valid () const nothrow @safe @nogc {
358 pragma(inline, true);
359 import core.stdc.math : isnan;
360 static if (dims == 2) return !isnan(x) && !isnan(y);
361 else static if (dims == 3) return !isnan(x) && !isnan(y) && !isnan(z);
362 else static assert(0, "invalid dimension count for vector");
364 // this also reject nans
365 pragma(inline
, true);
366 import core
.stdc
.math
: isfinite
;
367 static if (dims
== 2) return isfinite(x
) && isfinite(y
);
368 else static if (dims
== 3) return isfinite(x
) && isfinite(y
) && isfinite(z
);
369 else static assert(0, "invalid dimension count for vector");
372 // this also reject nans
373 @property bool isFinite () const nothrow @safe @nogc {
374 pragma(inline
, true);
375 import core
.stdc
.math
: isfinite
;
376 static if (dims
== 2) return isfinite(x
) && isfinite(y
);
377 else static if (dims
== 3) return isfinite(x
) && isfinite(y
) && isfinite(z
);
378 else static assert(0, "invalid dimension count for vector");
381 @property bool isZero () const nothrow @safe @nogc {
382 pragma(inline
, true);
383 static if (dims
== 2) return (x
== 0 && y
== 0);
384 else static if (dims
== 3) return (x
== 0 && y
== 0 && z
== 0);
385 else static assert(0, "invalid dimension count for vector");
388 @property bool isNearZero () const nothrow @safe @nogc {
389 pragma(inline
, true);
390 mixin(ImportCoreMath
!(Float
, "fabs"));
391 static if (dims
== 2) return (fabs(x
) < EPSILON
!Float
&& fabs(y
) < EPSILON
!Float
);
392 else static if (dims
== 3) return (fabs(x
) < EPSILON
!Float
&& fabs(y
) < EPSILON
!Float
&& fabs(z
) < EPSILON
!Float
);
393 else static assert(0, "invalid dimension count for vector");
396 void set (in Float
[] c
...) @trusted {
397 x
= (c
.length
>= 1 ? c
.ptr
[0] : 0);
398 y
= (c
.length
>= 2 ? c
.ptr
[1] : 0);
399 static if (dims
== 3) z
= (c
.length
>= 3 ? c
.ptr
[2] : 0);
402 static if (dims
== 2)
403 void set (in Float ax
, in Float ay
) {
404 //pragma(inline, true);
409 static if (dims
== 3)
410 void set (in Float ax
, in Float ay
, in Float az
) {
411 //pragma(inline, true);
417 void opAssign(VT
) (in auto ref VT v
) if (isVector
!VT
) {
418 //pragma(inline, true);
421 static if (dims
== 3) {
422 static if (isVector3
!VT
) z
= v
.z
; else z
= 0;
426 Float
opIndex (usize idx
) const {
427 pragma(inline
, true);
428 static if (dims
== 2) return (idx
== 0 ? x
: idx
== 1 ? y
: Float
.nan
);
429 else static if (dims
== 3) return (idx
== 0 ? x
: idx
== 1 ? y
: idx
== 2 ? z
: Float
.nan
);
430 else static assert(0, "invalid dimension count for vector");
433 void opIndexAssign (Float v
, usize idx
) {
434 pragma(inline
, true);
435 static if (dims
== 2) { if (idx
== 0) x
= v
; else if (idx
== 1) y
= v
; }
436 else static if (dims
== 3) { if (idx
== 0) x
= v
; else if (idx
== 1) y
= v
; else if (idx
== 2) z
= v
; }
437 else static assert(0, "invalid dimension count for vector");
440 ref auto normalize () {
441 //pragma(inline, true);
442 mixin(ImportCoreMath
!(double, "sqrt"));
443 version(vmath_slow_normalize
) {
444 static if (dims
== 2) immutable double len
= sqrt(x
*x
+y
*y
);
445 else static if (dims
== 3) immutable double len
= sqrt(x
*x
+y
*y
+z
*z
);
446 else static assert(0, "invalid dimension count for vector");
449 static if (dims
== 3) z
/= len
;
451 static if (dims
== 2) immutable double invlen
= 1.0/sqrt(x
*x
+y
*y
);
452 else static if (dims
== 3) immutable double invlen
= 1.0/sqrt(x
*x
+y
*y
+z
*z
);
453 else static assert(0, "invalid dimension count for vector");
456 static if (dims
== 3) z
*= invlen
;
461 Float
normalizeRetLength () {
462 //pragma(inline, true);
463 mixin(ImportCoreMath
!(double, "sqrt"));
464 static if (dims
== 2) immutable double len
= sqrt(x
*x
+y
*y
);
465 else static if (dims
== 3) immutable double len
= sqrt(x
*x
+y
*y
+z
*z
);
466 else static assert(0, "invalid dimension count for vector");
467 version(vmath_slow_normalize
) {
470 static if (dims
== 3) z
/= len
;
472 immutable double invlen
= 1.0/len
;
475 static if (dims
== 3) z
*= invlen
;
477 return cast(Float
)len
;
481 ref auto safeNormalize () pure {
482 //pragma(inline, true);
483 import std.math : sqrt;
484 static if (dims == 2) Float invlength = sqrt(x*x+y*y);
485 else static if (dims == 3) Float invlength = sqrt(x*x+y*y+z*z);
486 else static assert(0, "invalid dimension count for vector");
487 if (invlength >= EPSILON!Float) {
488 invlength = cast(Float)1/invlength;
491 static if (dims == 3) z *= invlength;
495 static if (dims == 3) z = 0;
501 ref auto opOpAssign(string op
, VT
) (in auto ref VT a
) if (isVector
!VT
&& (op
== "+" || op
== "-" || op
== "*")) {
502 //pragma(inline, true);
503 mixin("x "~op
~"= a.x;");
504 mixin("y "~op
~"= a.y;");
505 static if (dims
== 3 && isVector3
!VT
) mixin("z "~op
~"= a.z;");
509 ref auto opOpAssign(string op
) (Float a
) if (op
== "+" || op
== "-" || op
== "*") {
510 //pragma(inline, true);
511 mixin("x "~op
~"= a;");
512 mixin("y "~op
~"= a;");
513 static if (dims
== 3) mixin("z "~op
~"= a;");
517 ref auto opOpAssign(string op
:"/") (Float a
) {
518 //import std.math : abs;
519 //pragma(inline, true);
520 //a = (abs(a) >= EPSILON!Float ? 1.0/a : Float.nan);
521 version(all
/*vmath_slow_normalize*/) {
524 static if (dims
== 3) z
/= a
;
526 immutable double aa
= 1.0/a
;
529 static if (dims
== 3) z
*= aa
;
534 static if (dims
== 2) {
536 ref auto rotate (Float angle
) {
537 pragma(inline
, true);
538 mixin(ImportCoreMath
!(Float
, "cos", "sin"));
539 immutable Float c
= cos(angle
);
540 immutable Float s
= sin(angle
);
541 immutable Float nx
= x
*c
-y
*s
;
542 immutable Float ny
= x
*s
+y
*c
;
548 auto rotated (Float angle
) const {
549 pragma(inline
, true);
550 mixin(ImportCoreMath
!(Float
, "cos", "sin"));
551 immutable Float c
= cos(angle
);
552 immutable Float s
= sin(angle
);
553 return v2(x
*c
-y
*s
, x
*s
+y
*c
);
558 // from `this` to `a`
559 auto lerp(VT
) (in auto ref VT a
, in Float t
) if (isVector
!VT
) {
560 pragma(inline
, true);
561 return this+(a
-this)*t
;
565 pragma(inline
, true);
566 static if (dims
== 2) return v2(x
, y
).normalize
; else return v3(x
, y
, z
).normalize
;
570 auto safeNormalized () {
571 pragma(inline, true);
572 static if (dims == 2) return v2(x, y).safeNormalize; else return v3(x, y, z).safeNormalize;
576 @property Float
length () {
577 pragma(inline
, true);
578 mixin(ImportCoreMath
!(Float
, "sqrt"));
579 static if (dims
== 2) return sqrt(x
*x
+y
*y
);
580 else static if (dims
== 3) return sqrt(x
*x
+y
*y
+z
*z
);
581 else static assert(0, "invalid dimension count for vector");
584 @property double dbllength () {
585 pragma(inline
, true);
586 mixin(ImportCoreMath
!(double, "sqrt"));
587 static if (dims
== 2) return sqrt(cast(double)x
*cast(double)x
+cast(double)y
*cast(double)y
);
588 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
);
589 else static assert(0, "invalid dimension count for vector");
592 @property Float
lengthSquared () {
593 pragma(inline
, true);
594 static if (dims
== 2) return x
*x
+y
*y
;
595 else static if (dims
== 3) return x
*x
+y
*y
+z
*z
;
596 else static assert(0, "invalid dimension count for vector");
599 @property double dbllengthSquared () {
600 pragma(inline
, true);
601 static if (dims
== 2) return cast(double)x
*cast(double)x
+cast(double)y
*cast(double)y
;
602 else static if (dims
== 3) return cast(double)x
*cast(double)x
+cast(double)y
*cast(double)y
+cast(double)z
*cast(double)z
;
603 else static assert(0, "invalid dimension count for vector");
607 Float
distance(VT
) (in auto ref VT a
) if (isVector
!VT
) {
608 pragma(inline
, true);
609 mixin(ImportCoreMath
!(Float
, "sqrt"));
610 static if (dims
== 2) {
611 static if (isVector2
!VT
) return sqrt((x
-a
.x
)*(x
-a
.x
)+(y
-a
.y
)*(y
-a
.y
));
612 else static if (isVector3
!VT
) return sqrt((x
-a
.x
)*(x
-a
.x
)+(y
-a
.y
)*(y
-a
.y
)+a
.z
*a
.z
);
613 else static assert(0, "invalid dimension count for vector");
614 } else static if (dims
== 3) {
615 static if (isVector2
!VT
) return sqrt((x
-a
.x
)*(x
-a
.x
)+(y
-a
.y
)*(y
-a
.y
)+z
*z
);
616 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
));
617 else static assert(0, "invalid dimension count for vector");
619 static assert(0, "invalid dimension count for vector");
624 Float
distanceSquared(VT
) (in auto ref VT a
) if (isVector
!VT
) {
625 pragma(inline
, true);
626 static if (dims
== 2) {
627 static if (isVector2
!VT
) return (x
-a
.x
)*(x
-a
.x
)+(y
-a
.y
)*(y
-a
.y
);
628 else static if (isVector3
!VT
) return (x
-a
.x
)*(x
-a
.x
)+(y
-a
.y
)*(y
-a
.y
)+a
.z
*a
.z
;
629 else static assert(0, "invalid dimension count for vector");
630 } else static if (dims
== 3) {
631 static if (isVector2
!VT
) return (x
-a
.x
)*(x
-a
.x
)+(y
-a
.y
)*(y
-a
.y
)+z
*z
;
632 else static if (isVector3
!VT
) return (x
-a
.x
)*(x
-a
.x
)+(y
-a
.y
)*(y
-a
.y
)+(z
-a
.z
)*(z
-a
.z
);
633 else static assert(0, "invalid dimension count for vector");
635 static assert(0, "invalid dimension count for vector");
640 double dbldistance(VT
) (in auto ref VT a
) if (isVector
!VT
) {
641 pragma(inline
, true);
642 mixin(ImportCoreMath
!(double, "sqrt"));
643 return sqrt(dbldistanceSquared(a
));
647 double dbldistanceSquared(VT
) (in auto ref VT a
) if (isVector
!VT
) {
648 pragma(inline
, true);
649 static if (dims
== 2) {
650 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
);
651 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
;
652 else static assert(0, "invalid dimension count for vector");
653 } else static if (dims
== 3) {
654 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
;
655 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
);
656 else static assert(0, "invalid dimension count for vector");
658 static assert(0, "invalid dimension count for vector");
662 auto opBinary(string op
, VT
) (in auto ref VT a
) if (isVector
!VT
&& (op
== "+" || op
== "-")) {
663 pragma(inline
, true);
664 static if (dims
== 2 && isVector2
!VT
) mixin("return v2(x"~op
~"a.x, y"~op
~"a.y);");
665 else static if (dims
== 2 && isVector3
!VT
) mixin("return v3(x"~op
~"a.x, y"~op
~"a.y, 0);");
666 else static if (dims
== 3 && isVector2
!VT
) mixin("return v3(x"~op
~"a.x, y"~op
~"a.y, 0);");
667 else static if (dims
== 3 && isVector3
!VT
) mixin("return v3(x"~op
~"a.x, y"~op
~"a.y, z"~op
~"a.z);");
668 else static assert(0, "invalid dimension count for vector");
671 // vector elements operation
672 auto op(string opr
, VT
) (in auto ref VT a
) if (isVector
!VT
&& (opr
== "+" || opr
== "-" || opr
== "*" || opr
== "/")) {
673 pragma(inline
, true);
674 static if (dims
== 2) {
675 static if (isVector2
!VT
) mixin("return v2(x"~opr
~"a.x, y"~opr
~"a.y);");
676 else static if (isVector3
!VT
) mixin("return v2(x"~opr
~"a.x, y"~opr
~"a.y);");
677 else static assert(0, "invalid dimension count for vector");
678 } else static if (dims
== 3) {
679 static if (isVector2
!VT
) mixin("return v3(x"~opr
~"a.x, y"~opr
~"a.y, 0);");
680 else static if (isVector3
!VT
) mixin("return v3(x"~opr
~"a.x, y"~opr
~"a.y, z"~opr
~"a.z);");
681 else static assert(0, "invalid dimension count for vector");
683 static assert(0, "invalid dimension count for vector");
688 Float
opBinary(string op
:"*", VT
) (in auto ref VT a
) if (isVector
!VT
) { pragma(inline
, true); return dot(a
); }
691 auto opBinary(string op
:"%", VT
) (in auto ref VT a
) if (isVector
!VT
) { pragma(inline
, true); return cross(a
); }
692 auto opBinary(string op
:"^", VT
) (in auto ref VT a
) if (isVector
!VT
) { pragma(inline
, true); return cross(a
); }
694 static if (dims
== 2) {
695 auto opBinary(string op
:"%", VT
) (VT
.Float a
) if (isVector
!VT
) { pragma(inline
, true); return cross(a
); }
696 auto opBinary(string op
:"^", VT
) (VT
.Float a
) if (isVector
!VT
) { pragma(inline
, true); return cross(a
); }
699 auto opBinary(string op
) (Float a
) if (op
== "+" || op
== "-" || op
== "*") {
700 pragma(inline
, true);
701 static if (dims
== 2) mixin("return v2(x"~op
~"a, y"~op
~"a);");
702 else static if (dims
== 3) mixin("return v3(x"~op
~"a, y"~op
~"a, z"~op
~"a);");
703 else static assert(0, "invalid dimension count for vector");
706 auto opBinaryRight(string op
:"*") (Float a
) {
707 pragma(inline
, true);
708 static if (dims
== 2) mixin("return v2(x"~op
~"a, y"~op
~"a);");
709 else static if (dims
== 3) mixin("return v3(x"~op
~"a, y"~op
~"a, z"~op
~"a);");
710 else static assert(0, "invalid dimension count for vector");
713 auto opBinary(string op
:"/") (Float a
) {
714 pragma(inline
, true);
715 //import std.math : abs;
716 //immutable Float a = (abs(aa) >= EPSILON!Float ? 1.0/aa : Float.nan);
717 //immutable Float a = cast(Float)1/aa; // 1/0 == inf
718 version(all
/*vmath_slow_normalize*/) {
719 static if (dims
== 2) return v2(x
/a
, y
/a
);
720 else static if (dims
== 3) return v3(x
/a
, y
/a
, z
/a
);
721 else static assert(0, "invalid dimension count for vector");
723 a
= cast(Float
)1/a
; // 1/0 == inf
724 static if (dims
== 2) return v2(x
*a
, y
*a
);
725 else static if (dims
== 3) return v3(x
*a
, y
*a
, z
*a
);
726 else static assert(0, "invalid dimension count for vector");
730 auto opBinaryRight(string op
:"/") (Float aa
) {
731 pragma(inline
, true);
733 import std.math : abs;
734 static if (dims == 2) return v2((abs(x) >= EPSILON!Float ? aa/x : 0), (abs(y) >= EPSILON!Float ? aa/y : 0));
735 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));
736 else static assert(0, "invalid dimension count for vector");
738 static if (dims
== 2) return v2(aa
/x
, aa
/y
);
739 else static if (dims
== 3) return v3(aa
/x
, aa
/y
, aa
/z
);
740 else static assert(0, "invalid dimension count for vector");
743 auto opUnary(string op
:"+") () { pragma(inline
, true); return this; }
745 auto opUnary(string op
:"-") () {
746 pragma(inline
, true);
747 static if (dims
== 2) return v2(-x
, -y
);
748 else static if (dims
== 3) return v3(-x
, -y
, -z
);
749 else static assert(0, "invalid dimension count for vector");
752 // this method performs the following triple product: (this x b) x c
753 Me
tripleProduct() (in auto ref Me b
, in auto ref Me c
) {
755 static if (dims
== 2) {
757 immutable Float ac
= a
.x
*c
.x
+a
.y
*c
.y
;
759 immutable Float bc
= b
.x
*c
.x
+b
.y
*c
.y
;
760 // perform b * a.dot(c) - a * b.dot(c)
767 immutable Float ac
= a
.x
*c
.x
+a
.y
*c
.y
+a
.z
*c
.z
;
769 immutable Float bc
= b
.x
*c
.x
+b
.y
*c
.y
+b
.z
*c
.z
;
770 // perform b * a.dot(c) - a * b.dot(c)
780 pragma(inline
, true);
781 mixin(ImportCoreMath
!(Float
, "fabs"));
782 static if (dims
== 2) return v2(fabs(x
), fabs(y
));
783 else static if (dims
== 3) return v3(fabs(x
), fabs(y
), fabs(z
));
784 else static assert(0, "invalid dimension count for vector");
788 pragma(inline
, true);
789 static if (dims
== 2) return v2((x
< 0 ?
-1 : x
> 0 ?
1 : 0), (y
< 0 ?
-1 : y
> 0 ?
1 : 0));
790 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));
791 else static assert(0, "invalid dimension count for vector");
794 // `this` is edge; see glsl reference
795 auto step(VT
) (in auto ref VT val
) if (IsVector
!VT
) {
796 pragma(inline
, true);
797 static if (dims
== 2) return v2((val
.x
< this.x ?
0f : 1f), (val
.y
< this.y ?
0f : 1f));
798 else static if (dims
== 3) {
799 static if (VT
.Dims
== 3) {
800 return v3((val
.x
< this.x ?
0f : 1f), (val
.y
< this.y ?
0f : 1f), (val
.z
< this.z ?
0f : 1f));
802 return v3((val
.x
< this.x ?
0f : 1f), (val
.y
< this.y ?
0f : 1f), (0 < this.z ?
0f : 1f));
805 else static assert(0, "invalid dimension count for vector");
808 bool opEquals(VT
) (in auto ref VT a
) if (isVector
!VT
) {
809 pragma(inline
, true);
810 static if (dims
== 2 && isVector2
!VT
) return (x
== a
.x
&& y
== a
.y
);
811 else static if (dims
== 2 && isVector3
!VT
) return (x
== a
.x
&& y
== a
.y
&& a
.z
== 0);
812 else static if (dims
== 3 && isVector2
!VT
) return (x
== a
.x
&& y
== a
.y
&& z
== 0);
813 else static if (dims
== 3 && isVector3
!VT
) return (x
== a
.x
&& y
== a
.y
&& z
== a
.z
);
814 else static assert(0, "invalid dimension count for vector");
818 @property Float
dot(VT
) (in auto ref VT a
) if (isVector
!VT
) {
819 pragma(inline
, true);
820 static if (dims
== 2) {
822 } else static if (dims
== 3) {
823 static if (isVector2
!VT
) return x
*a
.x
+y
*a
.y
;
824 else static if (isVector3
!VT
) return x
*a
.x
+y
*a
.y
+z
*a
.z
;
825 else static assert(0, "invalid dimension count for vector");
827 static assert(0, "invalid dimension count for vector");
832 auto cross(VT
) (in auto ref VT a
) if (isVector
!VT
) {
833 pragma(inline
, true);
834 static if (dims
== 2 && isVector2
!VT
) return /*v3(0, 0, x*a.y-y*a.x)*/x
*a
.y
-y
*a
.x
;
835 else static if (dims
== 2 && isVector3
!VT
) return v3(y
*a
.z
, -x
*a
.z
, x
*a
.y
-y
*a
.x
);
836 else static if (dims
== 3 && isVector2
!VT
) return v3(-z
*a
.y
, z
*a
.x
, x
*a
.y
-y
*a
.x
);
837 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
);
838 else static assert(0, "invalid dimension count for vector");
841 // this*s; if you want s*this, do cross(-s)
842 static if (dims
== 2) auto cross (Float s
) {
843 pragma(inline
, true);
844 return Me(s
*y
, -s
*x
);
847 // compute Euler angles from direction vector (this) (with zero roll)
849 auto tmp
= this.normalized
;
850 /*hpr.x = -atan2(tmp.x, tmp.y);
851 hpr.y = -atan2(tmp.z, sqrt(tmp.x*tmp.x+tmp.y*tmp.y));*/
852 static if (dims
== 2) {
853 mixin(ImportCoreMath
!(double, "atan2"));
855 cast(Float
)(atan2(cast(double)tmp
.x
, cast(Float
)0)),
856 cast(Float
)(-atan2(cast(double)tmp
.y
, cast(double)tmp
.x
)),
859 mixin(ImportCoreMath
!(double, "atan2", "sqrt"));
861 cast(Float
)(atan2(cast(double)tmp
.x
, cast(double)tmp
.z
)),
862 cast(Float
)(-atan2(cast(double)tmp
.y
, cast(double)sqrt(tmp
.x
*tmp
.x
+tmp
.z
*tmp
.z
))),
868 // some more supplementary functions to support various things
869 Float
vcos(VT
) (in auto ref VT v
) if (isVector
!VT
) {
870 immutable double len
= length
*v
.length
;
871 return cast(Float
)(len
> EPSILON
!Float ?
cast(Float
)(dot(v
)/len
) : cast(Float
)0);
874 static if (dims
== 2) Float
vsin(VT
) (in auto ref VT v
) if (isVector
!VT
) {
875 immutable double len
= length
*v
.length
;
876 return cast(Float
)(len
> EPSILON
!Float ?
cast(Float
)(cross(v
)/len
) : cast(Float
)0);
879 static if (dims
== 2) Float
angle180(VT
) (in auto ref VT v
) if (isVector
!VT
) {
880 import std
.math
: PI
;
881 mixin(ImportCoreMath
!(Float
, "atan"));
882 immutable Float cosv
= vcos(v
);
883 immutable Float sinv
= vsin(v
);
884 if (cosv
== 0) return (sinv
<= 0 ?
-90 : 90);
885 if (sinv
== 0) return (cosv
<= 0 ?
180 : 0);
886 Float angle
= (cast(Float
)180*atan(sinv
/cosv
))/cast(Float
)PI
;
887 if (cosv
< 0) { if (angle
> 0) angle
-= 180; else angle
+= 180; }
891 static if (dims
== 2) Float
angle360(VT
) (in auto ref VT v
) if (isVector
!VT
) {
892 import std
.math
: PI
;
893 mixin(ImportCoreMath
!(Float
, "atan"));
894 immutable Float cosv
= vcos(v
);
895 immutable Float sinv
= vsin(v
);
896 if (cosv
== 0) return (sinv
<= 0 ?
270 : 90);
897 if (sinv
== 0) return (cosv
<= 0 ?
180 : 0);
898 Float angle
= (cast(Float
)180*atan(sinv
/cosv
))/cast(Float
)PI
;
899 if (cosv
< 0) angle
+= 180;
900 if (angle
< 0) angle
+= 360;
904 Float
relativeAngle(VT
) (in auto ref VT v
) if (isVector
!VT
) {
905 import std
.math
: PI
;
906 mixin(ImportCoreMath
!(Float
, "acos"));
907 immutable Float cosv
= vcos(v
);
908 if (cosv
<= -1) return PI
;
909 if (cosv
>= 1) return 0;
913 bool touch(VT
) (in auto ref VT v
) if (isVector
!VT
) { pragma(inline
, true); return (distance(v
) < /*SMALL*/EPSILON
!Float
); }
914 bool touch(VT
) (in auto ref VT v
, in Float epsilon
) if (isVector
!VT
) { pragma(inline
, true); return (distance(v
) < epsilon
); }
916 // is `this` on left? (or on line)
917 bool onLeft(VT
) (in auto ref VT v0
, in auto ref VT v1
) if (isVector
!VT
) {
918 pragma(inline
, true);
919 return ((v1
-v0
).cross(this-v0
) <= 0);
922 static if (dims
== 2) {
924 // test if a point (`this`) is left/on/right of an infinite 2D line
929 Float
side(VT
) (in auto ref VT v0
, in auto ref VT v1
) const if (isVector2
!VT
) {
930 pragma(inline
, true);
931 return ((v1
.x
-v0
.x
)*(this.y
-v0
.y
)-(this.x
-v0
.x
)*(v1
.y
-v0
.y
));
936 bool inside(VT
) (in auto ref VT v0
, in auto ref VT v1
, in auto ref VT v2
) if (isVector
!VT
) {
937 if ((v1
-v0
).cross(this-v0
) <= 0) return false;
938 if ((v2
-v1
).cross(this-v1
) <= 0) return false;
939 return ((v0
-v2
).cross(this-v2
) > 0);
942 // box2dlite port support
943 static if (dims
== 2) {
944 // returns a perpendicular vector (90 degree rotation)
945 auto perp() () { pragma(inline
, true); return v2(-y
, x
); }
947 // returns a perpendicular vector (-90 degree rotation)
948 auto rperp() () { pragma(inline
, true); return v2(y
, -x
); }
950 // returns the vector projection of this onto v
951 auto projectTo() (in auto ref v2 v
) { pragma(inline
, true); return v
*(this.dot(v
)/v
.dot(v
)); }
953 // returns the unit length vector for the given angle (in radians)
954 auto forAngle (in Float a
) { pragma(inline
, true); mixin(ImportCoreMath
!(Float
, "cos", "sin")); return v2(cos(a
), sin(a
)); }
956 // returns the angular direction v is pointing in (in radians)
957 Float
toAngle() () { pragma(inline
, true); mixin(ImportCoreMath
!(Float
, "atan2")); return atan2(y
, x
); }
959 auto scross() (Float s
) { pragma(inline
, true); return v2(-s
*y
, s
*x
); }
961 // returns the closest point to `this` on the line segment from `a` to `b`, or invalid vector
962 // if `asseg` is false, "segment" is actually a line (infinite in both directions)
963 Me
projectToSeg(bool asseg
=true) (in auto ref Me a
, in auto ref Me b
) {
964 mixin(ImportCoreMath
!(Float
, "fabs"));
966 immutable ab
= b
-a
; // vector from a to b
967 // squared distance from a to b
968 immutable absq
= ab
.dot(ab
);
969 if (fabs(absq
) < Epsilon
) return a
; // a and b are the same point (roughly)
970 immutable ap
= p
-a
; // vector from a to p
971 immutable t
= ap
.dot(ab
)/absq
;
973 if (t
< 0) return a
; // "before" a on the line
974 if (t
> 1) return b
; // "after" b on the line
976 // projection lies "inbetween" a and b on the line
980 // returns the closest point to `this` on the line segment from `a` to `b`, or invalid vector
981 // if `asseg` is false, "segment" is actually a line (infinite in both directions)
982 Me
projectToSegT(bool asseg
=true) (in auto ref Me a
, in auto ref Me b
, ref Float rest
) {
983 mixin(ImportCoreMath
!(Float
, "fabs"));
985 immutable ab
= b
-a
; // vector from a to b
986 // squared distance from a to b
987 immutable absq
= ab
.dot(ab
);
988 if (fabs(absq
) < Epsilon
) { rest
= 0; return a
; } // a and b are the same point (roughly)
989 immutable ap
= p
-a
; // vector from a to p
990 immutable t
= ap
.dot(ab
)/absq
;
993 if (t
< 0) return a
; // "before" a on the line
994 if (t
> 1) return b
; // "after" b on the line
996 // projection lies "inbetween" a and b on the line
1000 // returns `t` (normalized distance of projected point from `a`)
1001 Float
projectToLineT() (in auto ref Me a
, in auto ref Me b
) {
1002 mixin(ImportCoreMath
!(Float
, "fabs"));
1004 immutable ab
= b
-a
; // vector from a to b
1005 // squared distance from a to b
1006 immutable absq
= ab
.dot(ab
);
1007 if (fabs(absq
) < Epsilon
) return cast(Float
)0; // a and b are the same point (roughly)
1008 immutable ap
= p
-a
; // vector from a to p
1009 return cast(Float
)(ap
.dot(ab
)/absq
);
1013 bool equals() (in auto ref Me v
) {
1014 pragma(inline
, true);
1015 mixin(ImportCoreMath
!(Float
, "fabs"));
1016 static if (dims
== 2) return (fabs(x
-v
.x
) < EPSILON
!Float
&& fabs(y
-v
.y
) < EPSILON
!Float
);
1017 else static if (dims
== 3) return (fabs(x
-v
.x
) < EPSILON
!Float
&& fabs(y
-v
.y
) < EPSILON
!Float
&& fabs(z
-v
.z
) < EPSILON
!Float
);
1018 else static assert(0, "invalid dimension count for vector");
1023 auto opDispatch(string
fld) ()
1024 if ((dims
== 2 && isGoodSwizzling
!(fld, "xy", 2, 3)) ||
1025 (dims
== 3 && isGoodSwizzling
!(fld, "xyz", 2, 3)))
1027 static if (fld.length
== 2) {
1028 return mixin(SwizzleCtor
!("v2", fld));
1030 return mixin(SwizzleCtor
!("v3", fld));
1034 static if (dims
== 3) bool collinear() (in auto ref Me v1
, in auto ref Me v2
) {
1035 pragma(inline
, true);
1036 mixin(ImportCoreMath
!(Float
, "fabs"));
1038 immutable Float cx
= (v1
.y
-v0
.y
)*(v2
.z
-v0
.z
)-(v2
.y
-v0
.y
)*(v1
.z
-v0
.z
);
1039 immutable Float cy
= (v2
.x
-v0
.x
)*(v1
.z
-v0
.z
)-(v1
.x
-v0
.x
)*(v2
.z
-v0
.z
);
1040 immutable Float cz
= (v1
.x
-v0
.x
)*(v2
.y
-v0
.y
)-(v2
.x
-v0
.x
)*(v1
.y
-v0
.y
);
1041 return (fabs(cast(double)(cx
*cx
+cy
*cy
+cz
*cz
)) < EPSILON
!double);
1044 static if (dims
== 2) bool collinear() (in auto ref Me v1
, in auto ref Me v2
) {
1045 pragma(inline
, true);
1046 mixin(ImportCoreMath
!(double, "fabs"));
1048 immutable Float det
= cast(Float
)((v0
-v1
)*(v0
-v2
)-(v0
-v2
)*(v0
-v1
));
1049 return (fabs(det
) <= EPSILON
!Float
);
1052 static if (dims
== 2) {
1053 import std
.range
.primitives
: isInputRange
, ElementType
;
1055 static auto vertRange (const(Me
)[] varr
) {
1056 static struct VertRange
{
1059 nothrow @trusted @nogc:
1060 @property bool empty () const pure { pragma(inline
, true); return (pos
>= arr
.length
); }
1061 @property Me
front () const pure { pragma(inline
, true); return (pos
< arr
.length ? arr
.ptr
[pos
] : Me
.Invalid
); }
1062 void popFront () { pragma(inline
, true); if (pos
< arr
.length
) ++pos
; }
1063 auto save () const pure { pragma(inline
, true); return VertRange(arr
, pos
); }
1065 return VertRange(varr
, 0);
1068 bool insidePoly(VR
) (auto ref VR vr
) const if (isInputRange
!VR
&& is(ElementType
!VR
: const Me
)) {
1069 if (vr
.empty
) return false;
1072 if (vr
.empty
) return false;
1075 if (vr
.empty
) return false; //TODO: check if p is ON the edge?
1079 if (p
.y
> nmin(p1
.y
, p2
.y
)) {
1080 if (p
.y
<= nmax(p1
.y
, p2
.y
)) {
1081 if (p
.x
<= nmax(p1
.x
, p2
.x
)) {
1083 auto xinters
= (p
.y
-p1
.y
)*(p2
.x
-p1
.x
)/(p2
.y
-p1
.y
)+p1
.x
;
1084 if (p1
.x
== p2
.x || p
.x
<= xinters
) counter ^
= 1;
1089 if (vr
.empty
) break;
1094 return (counter
!= 0);
1097 bool insidePoly() (const(Me
)[] poly
) const { pragma(inline
, true); return insidePoly(vertRange(poly
)); }
1099 // gets the signed area
1100 // if the area is less than 0, it indicates that the polygon is clockwise winded
1101 Me
.Float
signedArea(VR
) (auto ref VR vr
) const if (isInputRange
!VR
&& is(ElementType
!VR
: const Me
)) {
1103 if (vr
.empty
) return area
;
1106 if (vr
.empty
) return area
;
1109 if (vr
.empty
) return area
;
1114 if (vr
.empty
) break;
1120 area
+= p2
.x
*pfirst
.y
;
1121 area
-= p2
.y
*pfirst
.x
;
1122 return area
/cast(VT
.Float
)2;
1125 Me
.Float
signedArea() (const(Me
)[] poly
) const { pragma(inline
, true); return signedArea(vertRange(poly
)); }
1127 // indicates if the vertices are in counter clockwise order
1128 // warning: If the area of the polygon is 0, it is unable to determine the winding
1129 bool isCCW(VR
) (auto ref VR vr
) const if (isInputRange
!VR
&& is(ElementType
!VR
: const Me
)) { pragma(inline
, true); return (signedArea(vr
) > 0); }
1130 bool isCCW() (const(Me
)[] poly
) const { pragma(inline
, true); return (signedArea(vertRange(poly
)) > 0); }
1132 // *signed* area; can be used to check on which side `b` is
1133 static auto triSignedArea() (in auto ref Me a
, in auto ref Me b
, in auto ref Me c
) {
1134 pragma(inline
, true);
1135 return (b
.x
-a
.x
)*(c
.y
-a
.y
)-(c
.x
-a
.x
)*(b
.y
-a
.y
);
1139 static if (dims
== 3) {
1140 // project this point to the given line
1141 Me
projectToLine() (in auto ref Me p0
, in auto ref Me p1
) const {
1142 immutable Me d
= p1
-p0
;
1143 immutable Me
.Float t
= d
.dot(this-p0
)/d
.dot(d
);
1144 if (tout
!is null) *tout
= t
;
1148 // return "time" of projection
1149 Me
.Float
projectToLineTime() (in auto ref Me p0
, in auto ref Me p1
) const {
1150 immutable Me d
= p1
-p0
;
1151 return d
.dot(this-p0
)/d
.dot(d
);
1154 // calculate triangle normal
1155 static Me
triangleNormal() (in auto ref Me v0
, in auto ref Me v1
, in auto ref Me v2
) {
1156 mixin(ImportCoreMath
!(Me
.Float
, "fabs"));
1157 immutable Me cp
= (v1
-v0
).cross(v2
-v1
);
1158 immutable Me
.Float m
= cp
.length
;
1159 return (fabs(m
) > Epsilon ? cp
*(cast(Me
.Float
)1/m
) : Me
.init
);
1162 // polygon must be convex, ccw, and without collinear vertices
1164 bool insideConvexPoly (const(Me
)[] ply
...) const @trusted {
1165 if (ply
.length
< 3) return false;
1166 immutable normal
= triangleNormal(ply
.ptr
[0], ply
.ptr
[1], ply
.ptr
[2]);
1167 auto pidx
= ply
.length
-1;
1168 for (typeof(pidx
) cidx
= 0; cidx
< ply
.length
; pidx
= cidx
, ++cidx
) {
1169 immutable pp1
= ply
.ptr
[pidx
];
1170 immutable pp2
= ply
.ptr
[cidx
];
1171 immutable side
= (pp2
-pp1
).cross(this-pp1
);
1172 if (normal
.dot(side
) < 0) return false;
1179 // linearly interpolate between v1 and v2
1181 VT lerp(VT) (in auto ref VT v1, in auto ref VT v2, const Float t) if (isSameVector!VT) {
1182 pragma(inline, true);
1183 return (v1*cast(Float)1.0f-t)+(v2*t);
1187 static if (dims
== 2) {
1188 Me
lineIntersect() (in auto ref Me p1
, in auto ref Me p2
, in auto ref Me q1
, in auto ref Me q2
) {
1189 pragma(inline
, true);
1190 mixin(ImportCoreMath
!(Me
.Float
, "fabs"));
1191 immutable Me
.Float a1
= p2
.y
-p1
.y
;
1192 immutable Me
.Float b1
= p1
.x
-p2
.x
;
1193 immutable Me
.Float c1
= a1
*p1
.x
+b1
*p1
.y
;
1194 immutable Me
.Float a2
= q2
.y
-q1
.y
;
1195 immutable Me
.Float b2
= q1
.x
-q2
.x
;
1196 immutable Me
.Float c2
= a2
*q1
.x
+b2
*q1
.y
;
1197 immutable Me
.Float det
= a1
*b2
-a2
*b1
;
1198 if (fabs(det
) > EPSILON
!(Me
.Float
)) {
1199 // lines are not parallel
1200 immutable Me
.Float invdet
= cast(Me
.Float
)1/det
;
1201 return Me((b2
*c1
-b1
*c2
)*invdet
, (a1
*c2
-a2
*c1
)*invdet
);
1206 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
) {
1207 mixin(ImportCoreMath
!(Me
.Float
, "fabs"));
1208 static if (firstIsSeg
&& secondIsSeg
) {
1209 // fast aabb test for possible early exit
1210 if (nmax(point0
.x
, point1
.x
) < nmin(point2
.x
, point3
.x
) ||
nmax(point2
.x
, point3
.x
) < nmin(point0
.x
, point1
.x
)) return Me
.Invalid
;
1211 if (nmax(point0
.y
, point1
.y
) < nmin(point2
.y
, point3
.y
) ||
nmax(point2
.y
, point3
.y
) < nmin(point0
.y
, point1
.y
)) return Me
.Invalid
;
1213 immutable Me
.Float den
= ((point3
.y
-point2
.y
)*(point1
.x
-point0
.x
))-((point3
.x
-point2
.x
)*(point1
.y
-point0
.y
));
1214 if (fabs(den
) > EPSILON
!(Me
.Float
)) {
1215 immutable Me
.Float e
= point0
.y
-point2
.y
;
1216 immutable Me
.Float f
= point0
.x
-point2
.x
;
1217 immutable Me
.Float invden
= cast(Me
.Float
)1/den
;
1218 immutable Me
.Float ua
= (((point3
.x
-point2
.x
)*e
)-((point3
.y
-point2
.y
)*f
))*invden
;
1219 static if (firstIsSeg
) { if (ua
< 0 || ua
> 1) return Me
.Invalid
; }
1220 if (ua
>= 0 && ua
<= 1) {
1221 immutable Me
.Float ub
= (((point1
.x
-point0
.x
)*e
)-((point1
.y
-point0
.y
)*f
))*invden
;
1222 static if (secondIsSeg
) { if (ub
< 0 || ub
> 1) return Me
.Invalid
; }
1223 if (ua
!= 0 || ub
!= 0) return Me(point0
.x
+ua
*(point1
.x
-point0
.x
), point0
.y
+ua
*(point1
.y
-point0
.y
));
1229 // returns hittime; <0: no collision; 0: inside
1230 // WARNING! NOT REALLY TESTED, AND MAY BE INCORRECT!
1231 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
) {
1232 mixin(ImportCoreMath
!(Me
.Float
, "fabs", "sqrt"));
1233 if (v0
.equals(v1
)) return (pos
.distanceSquared(v0
) <= radii
*radii ?
0 : -1); // v0 and v1 are the same point; do "point inside circle" check
1234 immutable Me normal
= (v1
-v0
).perp
.normalized
;
1235 immutable Me
.Float D
= -normal
*((v0
+v1
)/2);
1236 immutable d0
= normal
.dot(pos
)+D
;
1237 if (fabs(d0
) <= radii
) return cast(Me
.Float
)0; // inside
1239 immutable p1
= pos
+vel
;
1240 immutable Me
.Float d1
= normal
.dot(p1
)+D
;
1241 if (d0
> radii
&& d1
< radii
) {
1242 Me
.Float t
= (d0
-radii
)/(d0
-d1
); // normalized time
1244 // project hitp point to segment
1248 immutable ab
= b
-a
; // vector from a to b
1249 // squared distance from a to b
1250 immutable absq
= ab
.dot(ab
);
1251 //assert(absq != 0); // a and b are the same point
1252 if (fabs(absq
) < Me
.Epsilon
) return -1; // a and b are the same point (roughly) -- SOMETHING IS VERY WRONG
1253 // t1 is projection "time" of p; [0..1]
1254 Me
.Float t1
= (p
-a
).dot(ab
)/absq
; //a+t1*ab -- projected point
1255 // is "contact center" lies on the seg?
1256 if (t1
>= 0 && t1
<= 1) return t
; // yes: this is clear hit
1257 // because i'm teh idiot, i'll just check ray-circle intersection for edge's capsue endpoints
1258 // ('cause if we'll turn edge into the capsule (v0,v1) with radius radii, we can use raycasting)
1259 // this is not entirely valid for segments much shorter than radius, but meh
1261 immutable Me eco
= (t1
< 0 ? a
: b
);
1262 immutable Me rpj
= eco
.projectToSegT
!false(pos
, p1
, ct1
);
1263 immutable Me
.Float dsq
= eco
.distanceSquared(rpj
);
1264 if (dsq
>= radii
*radii
) return -1; // endpoint may be on sphere or out of it, this is not interesting
1265 immutable Me
.Float
dt = sqrt(radii
*radii
-dsq
)/sqrt(vel
.x
*vel
.x
+vel
.y
*vel
.y
);
1277 // ////////////////////////////////////////////////////////////////////////// //
1278 // 2x2 matrix for fast 2D rotations and such
1279 alias mat22
= Mat22
!vec2
;
1281 align(1) struct Mat22(VT
) if (IsVectorDim
!(VT
, 2)) {
1284 alias Float
= VT
.Float
;
1285 alias mat22
= typeof(this);
1286 alias vec2
= VecN
!(3, Float
);
1287 alias Me
= typeof(this);
1289 nothrow @safe @nogc:
1291 // default is "not rotated"
1292 vec2 col1
= vec2(1, 0);
1293 vec2 col2
= vec2(0, 1);
1296 this (Float angle
) { pragma(inline
, true); set(angle
); }
1298 void set (Float angle
) {
1299 pragma(inline
, true);
1300 mixin(ImportCoreMath
!(Float
, "cos", "sin"));
1301 immutable Float c
= cos(angle
), s
= sin(angle
);
1302 col1
.x
= c
; col1
.y
= s
;
1303 col2
.x
= -s
; col2
.y
= c
;
1307 this() (in auto ref vec2 acol1
, in auto ref vec2 acol2
) { pragma(inline
, true); col1
= acol1
; col2
= acol2
; }
1309 static Me
Identity () { pragma(inline
, true); Me res
; return res
; }
1311 Me
transpose () const { pragma(inline
, true); return Me(vec2(col1
.x
, col2
.x
), vec2(col1
.y
, col2
.y
)); }
1313 Me
invert () const @trusted {
1314 pragma(inline
, true);
1315 immutable Float a
= col1
.x
, b
= col2
.x
, c
= col1
.y
, d
= col2
.y
;
1317 Float det
= a
*d
-b
*c
;
1318 assert(det
!= cast(Float
)0);
1319 det
= cast(Float
)1/det
;
1327 Me
opUnary(string op
:"+") () const { pragma(inline
, true); return this; }
1328 Me
opUnary(string op
:"-") () const { pragma(inline
, true); return Me(-col1
, -col2
); }
1330 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
); }
1331 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
); }
1333 Me
opBinary(string op
:"*") (Float s
) const { pragma(inline
, true); return Me(vec2(col1
*s
, col2
*s
)); }
1334 Me
opBinaryRight(string op
:"*") (Float s
) const { pragma(inline
, true); return Me(vec2(col1
*s
, col2
*s
)); }
1336 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);"); }
1337 Me
opBinary(string op
:"*") (in auto ref Me bm
) const { pragma(inline
, true); return Me(this*bm
.col1
, this*bm
.col2
); }
1339 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; }
1341 ref Me
opOpAssign(string op
:"*") (Float s
) const { pragma(inline
, true); col1
*= s
; col2
*= s
; }
1343 Me
abs() () { pragma(inline
, true); return Me(col1
.abs
, col2
.abs
); }
1345 // solves the system of linear equations: Ax = b
1346 vec2
solve() (in auto ref vec2 v
) const {
1347 immutable Float a
= col1
.x
, b
= col2
.x
, c
= col1
.y
, d
= col2
.y
;
1348 Float det
= a
*d
-b
*c
;
1349 assert(det
!= cast(Float
)0);
1350 det
= cast(Float
)1/det
;
1351 return vec2((d
*v
.x
-b
*v
.y
)*det
, (a
*v
.y
-c
*v
.x
)*det
);
1356 // ////////////////////////////////////////////////////////////////////////// //
1357 alias mat3
= Mat3
!vec2
; /// for 2D
1358 alias mat33
= Mat3
!vec3
; /// for 3D
1360 /// very simple (and mostly not tested) 3x3 matrix, for 2D/3D (depenting of parameterizing vector type)
1361 /// for 3D, 3x3 matrix cannot do translation
1362 align(1) struct Mat3(VT
) if (IsVectorDim
!(VT
, 2) || IsVectorDim
!(VT
, 3)) {
1365 alias Float
= VT
.Float
;
1366 alias m3
= typeof(this);
1367 static if (VT
.Dims
== 2) {
1370 enum ThreeD
= false;
1371 enum isVector2(VT
) = is(VT
== VecN
!(2, Float
));
1376 enum isVector3(VT
) = is(VT
== VecN
!(3, Float
));
1380 // 3x3 matrix components
1388 string
toString () const nothrow @trusted {
1389 import std
.string
: format
;
1391 return "0:[%g,%g,%g]\n3:[%g,%g,%g]\n6:[%g,%g,%g]".format(
1392 m
.ptr
[0], m
.ptr
[1], m
.ptr
[2],
1393 m
.ptr
[3], m
.ptr
[4], m
.ptr
[5],
1394 m
.ptr
[6], m
.ptr
[7], m
.ptr
[8],
1396 } catch (Exception
) {
1402 nothrow @trusted @nogc:
1403 this (const(Float
)[] vals
...) {
1404 pragma(inline
, true);
1405 if (vals
.length
>= 3*3) {
1406 m
.ptr
[0..9] = vals
.ptr
[0..9];
1408 // so `m3(1)`, for example, will create matrix filled with `1`
1409 if (vals
.length
== 1) {
1410 m
.ptr
[0..9] = vals
.ptr
[0];
1412 // still clear the matrix
1414 m
.ptr
[0..vals
.length
] = vals
[];
1419 this() (in auto ref m3 mt
) {
1420 pragma(inline
, true);
1424 Float
opIndex (usize x
, usize y
) const {
1425 pragma(inline
, true);
1426 return (x
< 3 && y
< 3 ? m
.ptr
[y
*3+x
] : Float
.nan
);
1429 void opIndexAssign (Float v
, usize x
, usize y
) {
1430 pragma(inline
, true);
1431 if (x
< 3 && y
< 3) m
.ptr
[y
*3+x
] = v
;
1434 @property bool isIdentity () const {
1435 pragma(inline
, true); // can we?
1437 m
.ptr
[0] == 1 && m
.ptr
[1] == 0 && m
.ptr
[2] == 0 &&
1438 m
.ptr
[3] == 0 && m
.ptr
[4] == 1 && m
.ptr
[5] == 0 &&
1439 m
.ptr
[6] == 0 && m
.ptr
[7] == 0 && m
.ptr
[8] == 1;
1442 auto opUnary(string op
:"+") () const { pragma(inline
, true); return this; }
1444 auto opUnary(string op
:"-") () const {
1445 pragma(inline
, true);
1447 -m
.ptr
[0], -m
.ptr
[1], -m
.ptr
[2],
1448 -m
.ptr
[3], -m
.ptr
[4], -m
.ptr
[5],
1449 -m
.ptr
[6], -m
.ptr
[7], -m
.ptr
[8],
1453 auto opBinary(string op
) (in auto ref m3 b
) const if (op
== "+" || op
== "-") {
1454 pragma(inline
, true);
1456 mixin("res.m.ptr[0] = m.ptr[0]"~op
~"b.m.ptr[0];");
1457 mixin("res.m.ptr[1] = m.ptr[1]"~op
~"b.m.ptr[1];");
1458 mixin("res.m.ptr[2] = m.ptr[2]"~op
~"b.m.ptr[2];");
1459 mixin("res.m.ptr[3] = m.ptr[3]"~op
~"b.m.ptr[3];");
1460 mixin("res.m.ptr[4] = m.ptr[4]"~op
~"b.m.ptr[4];");
1461 mixin("res.m.ptr[5] = m.ptr[5]"~op
~"b.m.ptr[5];");
1462 mixin("res.m.ptr[6] = m.ptr[6]"~op
~"b.m.ptr[6];");
1463 mixin("res.m.ptr[7] = m.ptr[7]"~op
~"b.m.ptr[7];");
1464 mixin("res.m.ptr[8] = m.ptr[8]"~op
~"b.m.ptr[8];");
1468 ref auto opOpAssign(string op
) (in auto ref m3 b
) if (op
== "+" || op
== "-") {
1469 pragma(inline
, true);
1470 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];");
1471 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];");
1472 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];");
1476 auto opBinary(string op
) (in Float b
) const if (op
== "*" || op
== "/") {
1477 pragma(inline
, true);
1479 mixin("res.m.ptr[0] = m.ptr[0]"~op
~"b;");
1480 mixin("res.m.ptr[1] = m.ptr[1]"~op
~"b;");
1481 mixin("res.m.ptr[2] = m.ptr[2]"~op
~"b;");
1482 mixin("res.m.ptr[3] = m.ptr[3]"~op
~"b;");
1483 mixin("res.m.ptr[4] = m.ptr[4]"~op
~"b;");
1484 mixin("res.m.ptr[5] = m.ptr[5]"~op
~"b;");
1485 mixin("res.m.ptr[6] = m.ptr[6]"~op
~"b;");
1486 mixin("res.m.ptr[7] = m.ptr[7]"~op
~"b;");
1487 mixin("res.m.ptr[8] = m.ptr[8]"~op
~"b;");
1491 auto opBinaryRight(string op
) (in Float b
) const if (op
== "*" || op
== "/") {
1492 pragma(inline
, true);
1494 mixin("res.m.ptr[0] = m.ptr[0]"~op
~"b;");
1495 mixin("res.m.ptr[1] = m.ptr[1]"~op
~"b;");
1496 mixin("res.m.ptr[2] = m.ptr[2]"~op
~"b;");
1497 mixin("res.m.ptr[3] = m.ptr[3]"~op
~"b;");
1498 mixin("res.m.ptr[4] = m.ptr[4]"~op
~"b;");
1499 mixin("res.m.ptr[5] = m.ptr[5]"~op
~"b;");
1500 mixin("res.m.ptr[6] = m.ptr[6]"~op
~"b;");
1501 mixin("res.m.ptr[7] = m.ptr[7]"~op
~"b;");
1502 mixin("res.m.ptr[8] = m.ptr[8]"~op
~"b;");
1506 ref auto opOpAssign(string op
) (in Float b
) if (op
== "*" || op
== "/") {
1507 pragma(inline
, true);
1508 mixin("m.ptr[0]"~op
~"=b; m.ptr[1]"~op
~"=b; m.ptr[2]"~op
~"=b;");
1509 mixin("m.ptr[3]"~op
~"=b; m.ptr[4]"~op
~"=b; m.ptr[5]"~op
~"=b;");
1510 mixin("m.ptr[6]"~op
~"=b; m.ptr[7]"~op
~"=b; m.ptr[8]"~op
~"=b;");
1514 static if (TwoD
) auto opBinary(string op
:"*") (in auto ref v2 v
) const {
1515 pragma(inline
, true);
1517 v
.x
*m
.ptr
[3*0+0]+v
.y
*m
.ptr
[3*1+0]+m
.ptr
[3*2+0],
1518 v
.x
*m
.ptr
[3*0+1]+v
.y
*m
.ptr
[3*1+1]+m
.ptr
[3*2+1],
1522 static if (ThreeD
) auto opBinary(string op
:"*") (in auto ref v3 v
) const {
1523 pragma(inline
, true);
1525 v
.x
*m
.ptr
[3*0+0]+v
.y
*m
.ptr
[3*1+0]+v
.z
*m
.ptr
[3*2+0],
1526 v
.x
*m
.ptr
[3*0+1]+v
.y
*m
.ptr
[3*1+1]+v
.z
*m
.ptr
[3*2+1],
1527 v
.x
*m
.ptr
[3*0+2]+v
.y
*m
.ptr
[3*1+2]+v
.z
*m
.ptr
[3*2+2],
1531 static if (TwoD
) auto opBinaryRight(string op
:"*") (in auto ref v2 v
) const { pragma(inline
, true); return this*v
; }
1532 static if (ThreeD
) auto opBinaryRight(string op
:"*") (in auto ref v3 v
) const { pragma(inline
, true); return this*v
; }
1534 auto opBinary(string op
:"*") (in auto ref m3 b
) const {
1535 //pragma(inline, true);
1537 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];
1538 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];
1539 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];
1540 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];
1541 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];
1542 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];
1543 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];
1544 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];
1545 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];
1549 // multiply vector by transposed matrix (TESTME!)
1550 static if (ThreeD
) auto transmul() (in auto ref v3 v
) const {
1551 pragma(inline
, true);
1553 v
.x
*m
.ptr
[3*0+0]+v
.y
*m
.ptr
[3*0+1]+v
.z
*m
.ptr
[3*0+2],
1554 v
.x
*m
.ptr
[3*1+0]+v
.y
*m
.ptr
[3*1+1]+v
.z
*m
.ptr
[3*1+2],
1555 v
.x
*m
.ptr
[3*2+0]+v
.y
*m
.ptr
[3*2+1]+v
.z
*m
.ptr
[3*2+2],
1559 // sum of the diagonal components
1560 Float
trace () const { pragma(inline
, true); return m
.ptr
[3*0+0]+m
.ptr
[3*1+1]+m
.ptr
[3*2+2]; }
1563 Float
det () const {
1564 pragma(inline
, true);
1566 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]);
1567 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]);
1568 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]);
1572 auto transposed () const {
1573 pragma(inline
, true);
1575 res
.m
.ptr
[3*0+0] = m
.ptr
[3*0+0];
1576 res
.m
.ptr
[3*0+1] = m
.ptr
[3*1+0];
1577 res
.m
.ptr
[3*0+2] = m
.ptr
[3*2+0];
1578 res
.m
.ptr
[3*1+0] = m
.ptr
[3*0+1];
1579 res
.m
.ptr
[3*1+1] = m
.ptr
[3*1+1];
1580 res
.m
.ptr
[3*1+2] = m
.ptr
[3*2+1];
1581 res
.m
.ptr
[3*2+0] = m
.ptr
[3*0+2];
1582 res
.m
.ptr
[3*2+1] = m
.ptr
[3*1+2];
1583 res
.m
.ptr
[3*2+2] = m
.ptr
[3*2+2];
1588 //pragma(inline, true);
1589 immutable mtp
= this.transposed
;
1592 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];
1593 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];
1594 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];
1595 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];
1596 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];
1597 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];
1598 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];
1599 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];
1600 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];
1602 res
.m
.ptr
[3*0+1] *= -1;
1603 res
.m
.ptr
[3*1+0] *= -1;
1604 res
.m
.ptr
[3*1+2] *= -1;
1605 res
.m
.ptr
[3*2+1] *= -1;
1607 return res
/this.det
;
1610 static if (ThreeD
) {
1611 ref m3
rotateX (Float angle
) {
1612 mixin(ImportCoreMath
!(Float
, "cos", "sin"));
1615 // get the sine and cosine of the rotation angle
1616 immutable Float s
= sin(angle
);
1617 immutable Float c
= cos(angle
);
1619 // calculate the new values of the six affected matrix entries
1620 immutable Float temp01
= c
*A
[0, 1]+s
*A
[0, 2];
1621 immutable Float temp11
= c
*A
[1, 1]+s
*A
[1, 2];
1622 immutable Float temp21
= c
*A
[2, 1]+s
*A
[2, 2];
1623 immutable Float temp02
= c
*A
[0, 2]-s
*A
[0, 1];
1624 immutable Float temp12
= c
*A
[1, 2]-s
*A
[1, 1];
1625 immutable Float temp22
= c
*A
[2, 2]-s
*A
[2, 1];
1627 // put the results back into A
1628 A
[0, 1] = temp01
; A
[0, 2] = temp02
;
1629 A
[1, 1] = temp11
; A
[1, 2] = temp12
;
1630 A
[2, 1] = temp21
; A
[2, 2] = temp22
;
1635 ref m3
rotateY (Float angle
) {
1636 mixin(ImportCoreMath
!(Float
, "cos", "sin"));
1639 // get the sine and cosine of the rotation angle
1640 immutable Float s
= sin(angle
);
1641 immutable Float c
= cos(angle
);
1643 // calculate the new values of the six affected matrix entries
1644 immutable Float temp00
= c
*A
[0, 0]+s
*A
[0, 2];
1645 immutable Float temp10
= c
*A
[1, 0]+s
*A
[1, 2];
1646 immutable Float temp20
= c
*A
[2, 0]+s
*A
[2, 2];
1647 immutable Float temp02
= c
*A
[0, 2]-s
*A
[0, 0];
1648 immutable Float temp12
= c
*A
[1, 2]-s
*A
[1, 0];
1649 immutable Float temp22
= c
*A
[2, 2]-s
*A
[2, 0];
1651 // put the results back into XformToChange
1652 A
[0, 0] = temp00
; A
[0, 2] = temp02
;
1653 A
[1, 0] = temp10
; A
[1, 2] = temp12
;
1654 A
[2, 0] = temp20
; A
[2, 2] = temp22
;
1659 ref m3
rotateZ (Float angle
) {
1660 import core
.stdc
.math
: cos
, sin
;
1663 // get the sine and cosine of the rotation angle
1664 immutable Float s
= sin(angle
);
1665 immutable Float c
= cos(angle
);
1667 // calculate the new values of the six affected matrix entries
1668 immutable Float temp00
= c
*A
[0, 0]+s
*A
[0, 1];
1669 immutable Float temp10
= c
*A
[1, 0]+s
*A
[1, 1];
1670 immutable Float temp20
= c
*A
[2, 0]+s
*A
[2, 1];
1671 immutable Float temp01
= c
*A
[0, 1]-s
*A
[0, 0];
1672 immutable Float temp11
= c
*A
[1, 1]-s
*A
[1, 0];
1673 immutable Float temp21
= c
*A
[2, 1]-s
*A
[2, 0];
1675 // put the results back into XformToChange
1676 A
[0, 0] = temp00
; A
[0, 1] = temp01
;
1677 A
[1, 0] = temp10
; A
[1, 1] = temp11
;
1678 A
[2, 0] = temp20
; A
[2, 1] = temp21
;
1685 auto Identity () { pragma(inline
, true); return m3(); }
1686 auto Zero () { pragma(inline
, true); return m3(0); }
1689 auto Rotate (in Float angle
) {
1690 pragma(inline
, true);
1691 mixin(ImportCoreMath
!(Float
, "cos", "sin"));
1692 immutable Float c
= cos(angle
);
1693 immutable Float s
= sin(angle
);
1695 res
.m
.ptr
[3*0+0] = c
; res
.m
.ptr
[3*0+1] = s
;
1696 res
.m
.ptr
[3*1+0] = -s
; res
.m
.ptr
[3*1+1] = c
;
1700 auto Scale (in Float sx
, in Float sy
) {
1701 pragma(inline
, true);
1703 res
.m
.ptr
[3*0+0] = sx
;
1704 res
.m
.ptr
[3*1+1] = sy
;
1708 auto Scale() (in auto ref v2 sc
) {
1709 pragma(inline
, true);
1711 res
.m
.ptr
[3*0+0] = sc
.x
;
1712 res
.m
.ptr
[3*1+1] = sc
.y
;
1716 auto Translate (in Float dx
, in Float dy
) {
1717 pragma(inline
, true);
1719 res
.m
.ptr
[3*2+0] = dx
;
1720 res
.m
.ptr
[3*2+1] = dy
;
1724 auto Translate() (in auto ref v2 v
) {
1725 pragma(inline
, true);
1727 res
.m
.ptr
[3*2+0] = v
.x
;
1728 res
.m
.ptr
[3*2+1] = v
.y
;
1733 static if (ThreeD
) {
1734 // make rotation matrix from given angles
1735 static auto Rotate() (in auto ref v3 angles
) {
1736 mixin(ImportCoreMath
!(Float
, "cos", "sin"));
1738 immutable Float cos_b
= cos(angles
[0]);
1739 immutable Float sin_b
= sin(angles
[0]);
1740 immutable Float cos_c
= cos(angles
[1]);
1741 immutable Float sin_c
= sin(angles
[1]);
1742 immutable Float cos_a
= cos(angles
[2]);
1743 immutable Float sin_a
= sin(angles
[2]);
1748 M
[0, 0] = cos_a
*cos_c
-sin_a
*sin_b
*sin_c
;
1749 M
[0, 1] = sin_a
*cos_c
+cos_a
*sin_b
*sin_c
;
1750 M
[0, 2] = cos_b
*sin_c
;
1752 // second matrix row
1753 M
[1, 0] = -sin_a
*cos_b
;
1754 M
[1, 1] = cos_a
*cos_b
;
1758 M
[2, 0] = -cos_a
*sin_c
-sin_a
*sin_b
*cos_c
;
1759 M
[2, 1] = -sin_a
*sin_c
+cos_a
*sin_b
*cos_c
;
1760 M
[2, 2] = cos_b
*cos_c
;
1765 static auto RotateX() (Float angle
) {
1771 static auto RotateY() (Float angle
) {
1777 static auto RotateZ() (Float angle
) {
1786 // ////////////////////////////////////////////////////////////////////////// //
1787 alias mat4
= Mat4
!vec3
;
1789 align(1) struct Mat4(VT
) if (IsVectorDim
!(VT
, 3)) {
1792 alias Float
= VT
.Float
;
1793 alias mat4
= typeof(this);
1794 alias vec3
= VecN
!(3, Float
);
1797 // OpenGL-compatible, row by row
1806 string
toString () const @trusted {
1807 import std
.string
: format
;
1809 return "0:[%g,%g,%g,%g]\n4:[%g,%g,%g,%g]\n8:[%g,%g,%g,%g]\nc:[%g,%g,%g,%g]".format(
1810 mt
.ptr
[ 0], mt
.ptr
[ 1], mt
.ptr
[ 2], mt
.ptr
[ 3],
1811 mt
.ptr
[ 4], mt
.ptr
[ 5], mt
.ptr
[ 6], mt
.ptr
[ 7],
1812 mt
.ptr
[ 8], mt
.ptr
[ 9], mt
.ptr
[10], mt
.ptr
[11],
1813 mt
.ptr
[12], mt
.ptr
[13], mt
.ptr
[14], mt
.ptr
[15],
1815 } catch (Exception
) {
1820 Float
opIndex (usize x
, usize y
) const @trusted @nogc { pragma(inline
, true); return (x
< 4 && y
< 4 ? mt
.ptr
[y
*4+x
] : Float
.nan
); }
1821 void opIndexAssign (Float v
, usize x
, usize y
) @trusted @nogc { pragma(inline
, true); if (x
< 4 && y
< 4) mt
.ptr
[y
*4+x
] = v
; }
1824 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
]; } }
1825 this() (in auto ref mat4 m
) { pragma(inline
, true); mt
.ptr
[0..16] = m
.mt
.ptr
[0..16]; }
1826 this() (in auto ref vec3 v
) {
1827 //mt.ptr[0..16] = 0;
1828 mt
.ptr
[0*4+0] = v
.x
;
1829 mt
.ptr
[1*4+1] = v
.y
;
1830 mt
.ptr
[2*4+2] = v
.z
;
1831 //mt.ptr[3*4+3] = 1; // just in case
1834 static mat4
Zero () { pragma(inline
, true); return mat4(0); }
1835 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(); }
1837 // multiply current OpenGL matrix with this one
1838 // templated, to not compile it if we won't use it
1839 void glMultiply() () const {
1840 pragma(inline
, true);
1842 static if (is(Float
== float)) {
1843 glMultMatrixf(mt
.ptr
);
1845 glMultMatrixd(mt
.ptr
);
1849 static mat4
GLRetrieveAny() (uint mode
) nothrow @trusted @nogc {
1850 pragma(inline
, true);
1853 static if (is(Float
== float)) {
1854 glGetFloatv(mode
, res
.mt
.ptr
);
1856 glGetDoublev(mode
, res
.mt
.ptr
);
1861 static mat4
GLRetrieveModelView() () nothrow @trusted @nogc { pragma(inline
, true); import iv
.glbinds
; return GLRetrieveAny(GL_MODELVIEW_MATRIX
); }
1862 static mat4
GLRetrieveProjection() () nothrow @trusted @nogc { pragma(inline
, true); import iv
.glbinds
; return GLRetrieveAny(GL_PROJECTION_MATRIX
); }
1863 static mat4
GLRetrieveTexture() () nothrow @trusted @nogc { pragma(inline
, true); import iv
.glbinds
; return GLRetrieveAny(GL_TEXTURE_MATRIX
); }
1864 static mat4
GLRetrieveColor() () nothrow @trusted @nogc { pragma(inline
, true); import iv
.glbinds
; return GLRetrieveAny(GL_COLOR_MATRIX
); }
1866 void glRetrieveAny() (uint mode
) nothrow @trusted @nogc {
1867 pragma(inline
, true);
1869 static if (is(Float
== float)) {
1870 glGetFloatv(mode
, mt
.ptr
);
1872 glGetDoublev(mode
, mt
.ptr
);
1876 void glRetrieveModelView() () nothrow @trusted @nogc { pragma(inline
, true); import iv
.glbinds
; glRetrieveAny(GL_MODELVIEW_MATRIX
); }
1877 void glRetrieveProjection() () nothrow @trusted @nogc { pragma(inline
, true); import iv
.glbinds
; glRetrieveAny(GL_PROJECTION_MATRIX
); }
1878 void glRetrieveTexture() () nothrow @trusted @nogc { pragma(inline
, true); import iv
.glbinds
; glRetrieveAny(GL_TEXTURE_MATRIX
); }
1879 void glRetrieveColor() () nothrow @trusted @nogc { pragma(inline
, true); import iv
.glbinds
; glRetrieveAny(GL_COLOR_MATRIX
); }
1881 @property bool isIdentity () const {
1882 pragma(inline
, true); // can we?
1884 mt
.ptr
[0] == 1 && mt
.ptr
[1] == 0 && mt
.ptr
[2] == 0 && mt
.ptr
[3] == 0 &&
1885 mt
.ptr
[4] == 0 && mt
.ptr
[5] == 1 && mt
.ptr
[6] == 0 && mt
.ptr
[7] == 0 &&
1886 mt
.ptr
[8] == 0 && mt
.ptr
[9] == 0 && mt
.ptr
[10] == 1 && mt
.ptr
[11] == 0 &&
1887 mt
.ptr
[12] == 0 && mt
.ptr
[13] == 0 && mt
.ptr
[14] == 0 && mt
.ptr
[15] == 1;
1890 @property inout(Float
)* glGetVUnsafe () inout nothrow @trusted @nogc { pragma(inline
, true); return cast(typeof(return))mt
.ptr
; }
1892 Float
[4] getRow (int idx
) const {
1893 Float
[4] res
= Float
.nan
;
1896 res
.ptr
[0] = mt
.ptr
[0];
1897 res
.ptr
[1] = mt
.ptr
[4];
1898 res
.ptr
[2] = mt
.ptr
[8];
1899 res
.ptr
[3] = mt
.ptr
[12];
1902 res
.ptr
[0] = mt
.ptr
[1];
1903 res
.ptr
[1] = mt
.ptr
[5];
1904 res
.ptr
[2] = mt
.ptr
[9];
1905 res
.ptr
[3] = mt
.ptr
[13];
1908 res
.ptr
[0] = mt
.ptr
[2];
1909 res
.ptr
[1] = mt
.ptr
[6];
1910 res
.ptr
[2] = mt
.ptr
[10];
1911 res
.ptr
[3] = mt
.ptr
[14];
1914 res
.ptr
[0] = mt
.ptr
[3];
1915 res
.ptr
[1] = mt
.ptr
[7];
1916 res
.ptr
[2] = mt
.ptr
[11];
1917 res
.ptr
[3] = mt
.ptr
[15];
1924 Float
[4] getCol (int idx
) const {
1925 Float
[4] res
= Float
.nan
;
1926 if (idx
>= 0 && idx
<= 3) res
= mt
.ptr
[idx
*4..idx
*5];
1930 // this is for camera matrices
1931 vec3
upVector () const { pragma(inline
, true); return vec3(mt
.ptr
[1], mt
.ptr
[5], mt
.ptr
[9]); }
1932 vec3
rightVector () const { pragma(inline
, true); return vec3(mt
.ptr
[0], mt
.ptr
[4], mt
.ptr
[8]); }
1933 vec3
forwardVector () const { pragma(inline
, true); return vec3(mt
.ptr
[2], mt
.ptr
[6], mt
.ptr
[10]); }
1935 private enum SinCosImportMixin
= q
{
1936 static if (is(Float
== float)) {
1937 import core
.stdc
.math
: cos
=cosf
, sin
=sinf
;
1939 import core
.stdc
.math
: cos
, sin
;
1943 static mat4
RotateX() (Float angle
) {
1944 mixin(SinCosImportMixin
);
1946 res
.mt
.ptr
[0*4+0] = cast(Float
)1;
1947 res
.mt
.ptr
[1*4+1] = cos(angle
);
1948 res
.mt
.ptr
[2*4+1] = -sin(angle
);
1949 res
.mt
.ptr
[1*4+2] = sin(angle
);
1950 res
.mt
.ptr
[2*4+2] = cos(angle
);
1951 res
.mt
.ptr
[3*4+3] = cast(Float
)1;
1955 static mat4
RotateY() (Float angle
) {
1956 mixin(SinCosImportMixin
);
1958 res
.mt
.ptr
[0*4+0] = cos(angle
);
1959 res
.mt
.ptr
[2*4+0] = sin(angle
);
1960 res
.mt
.ptr
[1*4+1] = cast(Float
)1;
1961 res
.mt
.ptr
[0*4+2] = -sin(angle
);
1962 res
.mt
.ptr
[2*4+2] = cos(angle
);
1963 res
.mt
.ptr
[3*4+3] = cast(Float
)1;
1967 static mat4
RotateZ() (Float angle
) {
1968 mixin(SinCosImportMixin
);
1970 res
.mt
.ptr
[0*4+0] = cos(angle
);
1971 res
.mt
.ptr
[1*4+0] = -sin(angle
);
1972 res
.mt
.ptr
[0*4+1] = sin(angle
);
1973 res
.mt
.ptr
[1*4+1] = cos(angle
);
1974 res
.mt
.ptr
[2*4+2] = cast(Float
)1;
1975 res
.mt
.ptr
[3*4+3] = cast(Float
)1;
1979 static mat4
Translate() (in auto ref vec3 v
) {
1981 res
.mt
.ptr
[0*4+0] = res
.mt
.ptr
[1*4+1] = res
.mt
.ptr
[2*4+2] = 1;
1982 res
.mt
.ptr
[3*4+0] = v
.x
;
1983 res
.mt
.ptr
[3*4+1] = v
.y
;
1984 res
.mt
.ptr
[3*4+2] = v
.z
;
1985 res
.mt
.ptr
[3*4+3] = 1;
1989 static mat4
TranslateNeg() (in auto ref vec3 v
) {
1991 res
.mt
.ptr
[0*4+0] = res
.mt
.ptr
[1*4+1] = res
.mt
.ptr
[2*4+2] = 1;
1992 res
.mt
.ptr
[3*4+0] = -v
.x
;
1993 res
.mt
.ptr
[3*4+1] = -v
.y
;
1994 res
.mt
.ptr
[3*4+2] = -v
.z
;
1995 res
.mt
.ptr
[3*4+3] = 1;
1999 static mat4
Scale() (in auto ref vec3 v
) {
2001 res
.mt
.ptr
[0*4+0] = v
.x
;
2002 res
.mt
.ptr
[1*4+1] = v
.y
;
2003 res
.mt
.ptr
[2*4+2] = v
.z
;
2004 res
.mt
.ptr
[3*4+3] = 1;
2008 static mat4
Rotate() (in auto ref vec3 v
) {
2009 auto mx
= mat4
.RotateX(v
.x
);
2010 auto my
= mat4
.RotateY(v
.y
);
2011 auto mz
= mat4
.RotateZ(v
.z
);
2015 static mat4
RotateDeg() (in auto ref vec3 v
) {
2016 auto mx
= mat4
.RotateX(v
.x
.deg2rad
);
2017 auto my
= mat4
.RotateY(v
.y
.deg2rad
);
2018 auto mz
= mat4
.RotateZ(v
.z
.deg2rad
);
2022 // for camera; x is pitch (up/down); y is yaw (left/right); z is roll (tilt)
2023 static mat4
RotateZXY() (in auto ref vec3 v
) {
2024 auto mx
= mat4
.RotateX(v
.x
);
2025 auto my
= mat4
.RotateY(v
.y
);
2026 auto mz
= mat4
.RotateZ(v
.z
);
2030 // for camera; x is pitch (up/down); y is yaw (left/right); z is roll (tilt)
2031 static mat4
RotateZXYDeg() (in auto ref vec3 v
) {
2032 auto mx
= mat4
.RotateX(v
.x
.deg2rad
);
2033 auto my
= mat4
.RotateY(v
.y
.deg2rad
);
2034 auto mz
= mat4
.RotateZ(v
.z
.deg2rad
);
2038 // same as `glFrustum()`
2039 static mat4
Frustum() (Float left
, Float right
, Float bottom
, Float top
, Float nearVal
, Float farVal
) nothrow @trusted @nogc {
2041 res
.mt
.ptr
[0] = 2*nearVal
/(right
-left
);
2042 res
.mt
.ptr
[5] = 2*nearVal
/(top
-bottom
);
2043 res
.mt
.ptr
[8] = (right
+left
)/(right
-left
);
2044 res
.mt
.ptr
[9] = (top
+bottom
)/(top
-bottom
);
2045 res
.mt
.ptr
[10] = -(farVal
+nearVal
)/(farVal
-nearVal
);
2046 res
.mt
.ptr
[11] = -1;
2047 res
.mt
.ptr
[14] = -(2*farVal
*nearVal
)/(farVal
-nearVal
);
2052 // same as `glOrtho()`
2053 static mat4
Ortho() (Float left
, Float right
, Float bottom
, Float top
, Float nearVal
, Float farVal
) nothrow @trusted @nogc {
2055 res
.mt
.ptr
[0] = 2/(right
-left
);
2056 res
.mt
.ptr
[5] = 2/(top
-bottom
);
2057 res
.mt
.ptr
[10] = -2/(farVal
-nearVal
);
2058 res
.mt
.ptr
[12] = -(right
+left
)/(right
-left
);
2059 res
.mt
.ptr
[13] = -(top
+bottom
)/(top
-bottom
);
2060 res
.mt
.ptr
[14] = -(farVal
+nearVal
)/(farVal
-nearVal
);
2064 // same as `gluPerspective()`
2065 // sets the frustum to perspective mode
2066 // fovY - Field of vision in degrees in the y direction
2067 // aspect - Aspect ratio of the viewport
2068 // zNear - The near clipping distance
2069 // zFar - The far clipping distance
2070 static mat4
Perspective() (Float fovY
, Float aspect
, Float zNear
, Float zFar
) nothrow @trusted @nogc {
2071 import std
.math
: PI
;
2072 mixin(ImportCoreMath
!(Float
, "tan"));
2073 immutable Float fH
= cast(Float
)(tan(fovY
/360*PI
)*zNear
);
2074 immutable Float fW
= cast(Float
)(fH
*aspect
);
2075 return frustum(-fW
, fW
, -fH
, fH
, zNear
, zFar
);
2079 static mat4
LookAtFucked() (in auto ref vec3 eye
, in auto ref vec3 center
, in auto ref vec3 up
) {
2080 // compute vector `N = EP-VRP` and normalize `N`
2081 vec3 n
= eye
-center
;
2084 // compute vector `V = UP-VRP`
2085 // make vector `V` orthogonal to `N` and normalize `V`
2087 immutable dp
= v
.dot(n
); //dp = (float)V3Dot(&v,&n);
2088 //v.x -= dp * n.x; v.y -= dp * n.y; v.z -= dp * n.z;
2092 // compute vector `U = V x N` (cross product)
2093 immutable vec3 u
= v
.cross(n
);
2095 // write the vectors `U`, `V`, and `N` as the first three rows of first, second, and third columns of transformation matrix
2098 m
.mt
.ptr
[0*4+0] = u
.x
;
2099 m
.mt
.ptr
[1*4+0] = u
.y
;
2100 m
.mt
.ptr
[2*4+0] = u
.z
;
2101 //m.mt.ptr[3*4+0] = 0;
2103 m
.mt
.ptr
[0*4+1] = v
.x
;
2104 m
.mt
.ptr
[1*4+1] = v
.y
;
2105 m
.mt
.ptr
[2*4+1] = v
.z
;
2106 //m.mt.ptr[3*4+1] = 0;
2108 m
.mt
.ptr
[0*4+2] = n
.x
;
2109 m
.mt
.ptr
[1*4+2] = n
.y
;
2110 m
.mt
.ptr
[2*4+2] = n
.z
;
2111 //m.mt.ptr[3*4+2] = 0;
2113 // compute the fourth row of transformation matrix to include the translation of `VRP` to the origin
2114 m
.mt
.ptr
[3*4+0] = -u
.x
*center
.x
-u
.y
*center
.y
-u
.z
*center
.z
;
2115 m
.mt
.ptr
[3*4+1] = -v
.x
*center
.x
-v
.y
*center
.y
-v
.z
*center
.z
;
2116 m
.mt
.ptr
[3*4+2] = -n
.x
*center
.x
-n
.y
*center
.y
-n
.z
*center
.z
;
2117 m
.mt
.ptr
[3*4+3] = 1;
2119 foreach (ref Float f
; m
.mt
[]) {
2120 mixin(ImportCoreMath
!(Float
, "fabs"));
2121 if (fabs(f
) < EPSILON
!Float
) f
= 0;
2127 // does `gluLookAt()`
2128 mat4
lookAt() (in auto ref vec3 eye
, in auto ref vec3 center
, in auto ref vec3 up
) const {
2129 mixin(ImportCoreMath
!(Float
, "sqrt"));
2132 Float
[3] x
= void, y
= void, z
= void;
2134 // make rotation matrix
2136 z
.ptr
[0] = eye
.x
-center
.x
;
2137 z
.ptr
[1] = eye
.y
-center
.y
;
2138 z
.ptr
[2] = eye
.z
-center
.z
;
2139 mag
= sqrt(z
.ptr
[0]*z
.ptr
[0]+z
.ptr
[1]*z
.ptr
[1]+z
.ptr
[2]*z
.ptr
[2]);
2149 // X vector = Y cross Z
2150 x
.ptr
[0] = y
.ptr
[1]*z
.ptr
[2]-y
.ptr
[2]*z
.ptr
[1];
2151 x
.ptr
[1] = -y
.ptr
[0]*z
.ptr
[2]+y
.ptr
[2]*z
.ptr
[0];
2152 x
.ptr
[2] = y
.ptr
[0]*z
.ptr
[1]-y
.ptr
[1]*z
.ptr
[0];
2153 // Recompute Y = Z cross X
2154 y
.ptr
[0] = z
.ptr
[1]*x
.ptr
[2]-z
.ptr
[2]*x
.ptr
[1];
2155 y
.ptr
[1] = -z
.ptr
[0]*x
.ptr
[2]+z
.ptr
[2]*x
.ptr
[0];
2156 y
.ptr
[2] = z
.ptr
[0]*x
.ptr
[1]-z
.ptr
[1]*x
.ptr
[0];
2158 /* cross product gives area of parallelogram, which is < 1.0 for
2159 * non-perpendicular unit-length vectors; so normalize x, y here
2161 mag
= sqrt(x
.ptr
[0]*x
.ptr
[0]+x
.ptr
[1]*x
.ptr
[1]+x
.ptr
[2]*x
.ptr
[2]);
2168 mag
= sqrt(y
.ptr
[0]*y
.ptr
[0]+y
.ptr
[1]*y
.ptr
[1]+y
.ptr
[2]*y
.ptr
[2]);
2175 m
.mt
.ptr
[0*4+0] = x
.ptr
[0];
2176 m
.mt
.ptr
[1*4+0] = x
.ptr
[1];
2177 m
.mt
.ptr
[2*4+0] = x
.ptr
[2];
2178 m
.mt
.ptr
[3*4+0] = 0;
2179 m
.mt
.ptr
[0*4+1] = y
.ptr
[0];
2180 m
.mt
.ptr
[1*4+1] = y
.ptr
[1];
2181 m
.mt
.ptr
[2*4+1] = y
.ptr
[2];
2182 m
.mt
.ptr
[3*4+1] = 0;
2183 m
.mt
.ptr
[0*4+2] = z
.ptr
[0];
2184 m
.mt
.ptr
[1*4+2] = z
.ptr
[1];
2185 m
.mt
.ptr
[2*4+2] = z
.ptr
[2];
2186 m
.mt
.ptr
[3*4+2] = 0;
2187 m
.mt
.ptr
[0*4+3] = 0;
2188 m
.mt
.ptr
[1*4+3] = 0;
2189 m
.mt
.ptr
[2*4+3] = 0;
2190 m
.mt
.ptr
[3*4+3] = 1;
2192 // move, and translate Eye to Origin
2193 return this*m
*Translate(-eye
);
2196 // rotate matrix to face along the target direction
2197 // this function will clear the previous rotation and scale, but it will keep the previous translation
2198 // it is for rotating object to look at the target, NOT for camera
2199 ref mat4
lookingAt() (in auto ref vec3 target
) {
2200 mixin(ImportCoreMath
!(Float
, "fabs"));
2201 vec3 position
= vec3(mt
.ptr
[12], mt
.ptr
[13], mt
.ptr
[14]);
2202 vec3 forward
= (target
-position
).normalized
;
2204 if (fabs(forward
.x
) < EPSILON
!Float
&& fabs(forward
.z
) < EPSILON
!Float
) {
2205 up
.z
= (forward
.y
> 0 ?
-1 : 1);
2209 vec3 left
= up
.cross(forward
).normalized
;
2210 up
= forward
.cross(left
).normalized
; //k8: `normalized` was commented out; why?
2211 mt
.ptr
[0*4+0] = left
.x
;
2212 mt
.ptr
[0*4+1] = left
.y
;
2213 mt
.ptr
[0*4+2] = left
.z
;
2214 mt
.ptr
[1*4+0] = up
.x
;
2215 mt
.ptr
[1*4+1] = up
.y
;
2216 mt
.ptr
[1*4+2] = up
.z
;
2217 mt
.ptr
[2*4+0] = forward
.x
;
2218 mt
.ptr
[2*4+1] = forward
.y
;
2219 mt
.ptr
[2*4+2] = forward
.z
;
2223 ref mat4
lookingAt() (in auto ref vec3 target
, in auto ref vec3 upVec
) {
2224 vec3 position
= vec3(mt
.ptr
[12], mt
.ptr
[13], mt
.ptr
[14]);
2225 vec3 forward
= (target
-position
).normalized
;
2226 vec3 left
= upVec
.cross(forward
).normalized
;
2227 vec3 up
= forward
.cross(left
).normalized
;
2228 mt
.ptr
[0*4+0] = left
.x
;
2229 mt
.ptr
[0*4+1] = left
.y
;
2230 mt
.ptr
[0*4+2] = left
.z
;
2231 mt
.ptr
[1*4+0] = up
.x
;
2232 mt
.ptr
[1*4+1] = up
.y
;
2233 mt
.ptr
[1*4+2] = up
.z
;
2234 mt
.ptr
[2*4+0] = forward
.x
;
2235 mt
.ptr
[2*4+1] = forward
.y
;
2236 mt
.ptr
[2*4+2] = forward
.z
;
2240 mat4
lookAt() (in auto ref vec3 target
) { pragma(inline
, true); auto res
= mat4(this); return this.lookingAt(target
); }
2241 mat4
lookAt() (in auto ref vec3 target
, in auto ref vec3 upVec
) { pragma(inline
, true); auto res
= mat4(this); return this.lookingAt(target
, upVec
); }
2243 ref mat4
rotate() (Float angle
, in auto ref vec3 axis
) {
2244 mixin(SinCosImportMixin
);
2245 angle
= deg2rad(angle
);
2246 immutable Float c
= cos(angle
);
2247 immutable Float s
= sin(angle
);
2248 immutable Float c1
= 1-c
;
2249 immutable Float m0
= mt
.ptr
[0], m4
= mt
.ptr
[4], m8
= mt
.ptr
[8], m12
= mt
.ptr
[12];
2250 immutable Float m1
= mt
.ptr
[1], m5
= mt
.ptr
[5], m9
= mt
.ptr
[9], m13
= mt
.ptr
[13];
2251 immutable Float m2
= mt
.ptr
[2], m6
= mt
.ptr
[6], m10
= mt
.ptr
[10], m14
= mt
.ptr
[14];
2253 // build rotation matrix
2254 immutable Float r0
= axis
.x
*axis
.x
*c1
+c
;
2255 immutable Float r1
= axis
.x
*axis
.y
*c1
+axis
.z
*s
;
2256 immutable Float r2
= axis
.x
*axis
.z
*c1
-axis
.y
*s
;
2257 immutable Float r4
= axis
.x
*axis
.y
*c1
-axis
.z
*s
;
2258 immutable Float r5
= axis
.y
*axis
.y
*c1
+c
;
2259 immutable Float r6
= axis
.y
*axis
.z
*c1
+axis
.x
*s
;
2260 immutable Float r8
= axis
.x
*axis
.z
*c1
+axis
.y
*s
;
2261 immutable Float r9
= axis
.y
*axis
.z
*c1
-axis
.x
*s
;
2262 immutable Float r10
= axis
.z
*axis
.z
*c1
+c
;
2264 // multiply rotation matrix
2265 mt
.ptr
[0] = r0
*m0
+r4
*m1
+r8
*m2
;
2266 mt
.ptr
[1] = r1
*m0
+r5
*m1
+r9
*m2
;
2267 mt
.ptr
[2] = r2
*m0
+r6
*m1
+r10
*m2
;
2268 mt
.ptr
[4] = r0
*m4
+r4
*m5
+r8
*m6
;
2269 mt
.ptr
[5] = r1
*m4
+r5
*m5
+r9
*m6
;
2270 mt
.ptr
[6] = r2
*m4
+r6
*m5
+r10
*m6
;
2271 mt
.ptr
[8] = r0
*m8
+r4
*m9
+r8
*m10
;
2272 mt
.ptr
[9] = r1
*m8
+r5
*m9
+r9
*m10
;
2273 mt
.ptr
[10] = r2
*m8
+r6
*m9
+r10
*m10
;
2274 mt
.ptr
[12] = r0
*m12
+r4
*m13
+r8
*m14
;
2275 mt
.ptr
[13] = r1
*m12
+r5
*m13
+r9
*m14
;
2276 mt
.ptr
[14] = r2
*m12
+r6
*m13
+r10
*m14
;
2281 ref mat4
rotateX() (Float angle
) {
2282 mixin(SinCosImportMixin
);
2283 angle
= deg2rad(angle
);
2284 immutable Float c
= cos(angle
);
2285 immutable Float s
= sin(angle
);
2286 immutable Float m1
= mt
.ptr
[1], m2
= mt
.ptr
[2];
2287 immutable Float m5
= mt
.ptr
[5], m6
= mt
.ptr
[6];
2288 immutable Float m9
= mt
.ptr
[9], m10
= mt
.ptr
[10];
2289 immutable Float m13
= mt
.ptr
[13], m14
= mt
.ptr
[14];
2291 mt
.ptr
[1] = m1
*c
+m2
*-s
;
2292 mt
.ptr
[2] = m1
*s
+m2
*c
;
2293 mt
.ptr
[5] = m5
*c
+m6
*-s
;
2294 mt
.ptr
[6] = m5
*s
+m6
*c
;
2295 mt
.ptr
[9] = m9
*c
+m10
*-s
;
2296 mt
.ptr
[10]= m9
*s
+m10
*c
;
2297 mt
.ptr
[13]= m13
*c
+m14
*-s
;
2298 mt
.ptr
[14]= m13
*s
+m14
*c
;
2303 ref mat4
rotateY() (Float angle
) {
2304 mixin(SinCosImportMixin
);
2305 angle
= deg2rad(angle
);
2306 immutable Float c
= cos(angle
);
2307 immutable Float s
= sin(angle
);
2308 immutable Float m0
= mt
.ptr
[0], m2
= mt
.ptr
[2];
2309 immutable Float m4
= mt
.ptr
[4], m6
= mt
.ptr
[6];
2310 immutable Float m8
= mt
.ptr
[8], m10
= mt
.ptr
[10];
2311 immutable Float m12
= mt
.ptr
[12], m14
= mt
.ptr
[14];
2313 mt
.ptr
[0] = m0
*c
+m2
*s
;
2314 mt
.ptr
[2] = m0
*-s
+m2
*c
;
2315 mt
.ptr
[4] = m4
*c
+m6
*s
;
2316 mt
.ptr
[6] = m4
*-s
+m6
*c
;
2317 mt
.ptr
[8] = m8
*c
+m10
*s
;
2318 mt
.ptr
[10]= m8
*-s
+m10
*c
;
2319 mt
.ptr
[12]= m12
*c
+m14
*s
;
2320 mt
.ptr
[14]= m12
*-s
+m14
*c
;
2325 ref mat4
rotateZ() (Float angle
) {
2326 mixin(SinCosImportMixin
);
2327 angle
= deg2rad(angle
);
2328 immutable Float c
= cos(angle
);
2329 immutable Float s
= sin(angle
);
2330 immutable Float m0
= mt
.ptr
[0], m1
= mt
.ptr
[1];
2331 immutable Float m4
= mt
.ptr
[4], m5
= mt
.ptr
[5];
2332 immutable Float m8
= mt
.ptr
[8], m9
= mt
.ptr
[9];
2333 immutable Float m12
= mt
.ptr
[12], m13
= mt
.ptr
[13];
2335 mt
.ptr
[0] = m0
*c
+m1
*-s
;
2336 mt
.ptr
[1] = m0
*s
+m1
*c
;
2337 mt
.ptr
[4] = m4
*c
+m5
*-s
;
2338 mt
.ptr
[5] = m4
*s
+m5
*c
;
2339 mt
.ptr
[8] = m8
*c
+m9
*-s
;
2340 mt
.ptr
[9] = m8
*s
+m9
*c
;
2341 mt
.ptr
[12]= m12
*c
+m13
*-s
;
2342 mt
.ptr
[13]= m12
*s
+m13
*c
;
2348 ref mat4
translate() (in auto ref vec3 v
) {
2349 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
;
2350 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
;
2351 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
;
2355 ref mat4
translateNeg() (in auto ref vec3 v
) {
2356 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
;
2357 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
;
2358 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
;
2363 ref mat4 translate() (in auto ref vec3 v) {
2370 ref mat4 translateNeg() (in auto ref vec3 v) {
2379 ref mat4
scale() (in auto ref vec3 v
) {
2380 mt
.ptr
[0] *= v
.x
; mt
.ptr
[4] *= v
.x
; mt
.ptr
[8] *= v
.x
; mt
.ptr
[12] *= v
.x
;
2381 mt
.ptr
[1] *= v
.y
; mt
.ptr
[5] *= v
.y
; mt
.ptr
[9] *= v
.y
; mt
.ptr
[13] *= v
.y
;
2382 mt
.ptr
[2] *= v
.z
; mt
.ptr
[6] *= v
.z
; mt
.ptr
[10] *= v
.z
; mt
.ptr
[14] *= v
.z
;
2386 mat4
rotated() (Float angle
, in auto ref vec3 axis
) const { pragma(inline
, true); auto res
= mat4(this); return res
.rotate(angle
, axis
); }
2387 mat4
rotatedX() (Float angle
) const { pragma(inline
, true); auto res
= mat4(this); return res
.rotateX(angle
); }
2388 mat4
rotatedY() (Float angle
) const { pragma(inline
, true); auto res
= mat4(this); return res
.rotateY(angle
); }
2389 mat4
rotatedZ() (Float angle
) const { pragma(inline
, true); auto res
= mat4(this); return res
.rotateZ(angle
); }
2390 mat4
translated() (in auto ref vec3 v
) const { pragma(inline
, true); auto res
= mat4(this); return res
.translate(v
); }
2391 mat4
translatedNeg() (in auto ref vec3 v
) const { pragma(inline
, true); auto res
= mat4(this); return res
.translateNeg(v
); }
2392 mat4
scaled() (in auto ref vec3 v
) const { pragma(inline
, true); auto res
= mat4(this); return res
.scale(v
); }
2394 // retrieve angles in degree from rotation matrix, M = Rx*Ry*Rz, in degrees
2395 // Rx: rotation about X-axis, pitch
2396 // Ry: rotation about Y-axis, yaw (heading)
2397 // Rz: rotation about Z-axis, roll
2398 vec3
getAnglesDeg () const {
2399 mixin(ImportCoreMath
!(Float
, "asin", "atan2"));
2400 Float pitch
= void, roll
= void;
2401 Float yaw
= rad2deg(asin(mt
.ptr
[8]));
2402 if (mt
.ptr
[10] < 0) {
2403 if (yaw
>= 0) yaw
= 180-yaw
; else yaw
= -180-yaw
;
2405 if (mt
.ptr
[0] > -EPSILON
!Float
&& mt
.ptr
[0] < EPSILON
!Float
) {
2407 pitch
= rad2deg(atan2(mt
.ptr
[1], mt
.ptr
[5]));
2409 roll
= rad2deg(atan2(-mt
.ptr
[4], mt
.ptr
[0]));
2410 pitch
= rad2deg(atan2(-mt
.ptr
[9], mt
.ptr
[10]));
2412 return vec3(pitch
, yaw
, roll
);
2415 // retrieve angles in degree from rotation matrix, M = Rx*Ry*Rz, in radians
2416 // Rx: rotation about X-axis, pitch
2417 // Ry: rotation about Y-axis, yaw (heading)
2418 // Rz: rotation about Z-axis, roll
2419 vec3
getAngles () const {
2420 mixin(ImportCoreMath
!(Float
, "asin", "atan2"));
2421 Float pitch
= void, roll
= void;
2422 Float yaw
= asin(mt
.ptr
[8]);
2423 if (mt
.ptr
[10] < 0) {
2424 if (yaw
>= 0) yaw
= 180-yaw
; else yaw
= -180-yaw
;
2426 if (mt
.ptr
[0] > -EPSILON
!Float
&& mt
.ptr
[0] < EPSILON
!Float
) {
2428 pitch
= atan2(mt
.ptr
[1], mt
.ptr
[5]);
2430 roll
= atan2(-mt
.ptr
[4], mt
.ptr
[0]);
2431 pitch
= atan2(-mt
.ptr
[9], mt
.ptr
[10]);
2433 return vec3(pitch
, yaw
, roll
);
2436 vec3
opBinary(string op
:"*") (in auto ref vec3 v
) const {
2437 //pragma(inline, true);
2439 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],
2440 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],
2441 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]);
2444 vec3
opBinaryRight(string op
:"*") (in auto ref vec3 v
) const {
2445 //pragma(inline, true);
2447 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],
2448 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],
2449 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]);
2452 mat4
opBinary(string op
:"*") (in auto ref mat4 m
) const {
2453 //pragma(inline, true);
2455 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],
2456 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],
2457 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],
2458 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],
2462 mat4
opBinary(string op
:"+") (in auto ref mat4 m
) const {
2463 auto res
= mat4(this);
2468 mat4
opBinary(string op
:"*") (Float a
) const {
2469 auto res
= mat4(this);
2474 mat4
opBinary(string op
:"/") (Float a
) const {
2475 mixin(ImportCoreMath
!(Float
, "fabs"));
2476 auto res
= mat4(this);
2477 if (fabs(a
) >= FLTEPS
) {
2486 ref vec2
opOpAssign(string op
:"*") (in auto ref mat4 m
) {
2488 foreach (immutable i
; 0..4) {
2489 foreach (immutable j
; 0..4) {
2490 foreach (immutable k
; 0..4) {
2491 res
.mt
.ptr
[i
+j
*4] += mt
.ptr
[i
+k
*4]*m
.mt
.ptr
[k
+j
*4];
2499 mat4
transposed () const {
2502 foreach (immutable i; 0..4) {
2503 foreach (immutable j; 0..4) {
2504 res.mt.ptr[i+j*4] = mt.ptr[j+i*4];
2510 mt
.ptr
[0], mt
.ptr
[4], mt
.ptr
[8], mt
.ptr
[12],
2511 mt
.ptr
[1], mt
.ptr
[5], mt
.ptr
[9], mt
.ptr
[13],
2512 mt
.ptr
[2], mt
.ptr
[6], mt
.ptr
[10], mt
.ptr
[14],
2513 mt
.ptr
[3], mt
.ptr
[7], mt
.ptr
[11], mt
.ptr
[15],
2517 void negate () { foreach (ref v
; mt
) v
= -v
; }
2519 mat4
opUnary(string op
:"-") () const { pragma(inline
, true); return this; }
2521 mat4
opUnary(string op
:"-") () const {
2523 -mt
.ptr
[0], -mt
.ptr
[1], -mt
.ptr
[2], -mt
.ptr
[3],
2524 -mt
.ptr
[4], -mt
.ptr
[5], -mt
.ptr
[6], -mt
.ptr
[7],
2525 -mt
.ptr
[8], -mt
.ptr
[9], -mt
.ptr
[10], -mt
.ptr
[11],
2526 -mt
.ptr
[12], -mt
.ptr
[13], -mt
.ptr
[14], -mt
.ptr
[15],
2530 // blends two matrices together, at a given percentage (range is [0..1]), blend==0: m2 is ignored
2531 // WARNING! won't sanitize `blend`
2532 mat4
blended() (in auto ref mat4 m2
, Float blend
) const {
2533 immutable Float ib
= cast(Float
)1-blend
;
2535 res
.mt
.ptr
[0] = mt
.ptr
[0]*ib
+m2
.mt
.ptr
[0]*blend
;
2536 res
.mt
.ptr
[1] = mt
.ptr
[1]*ib
+m2
.mt
.ptr
[1]*blend
;
2537 res
.mt
.ptr
[2] = mt
.ptr
[2]*ib
+m2
.mt
.ptr
[2]*blend
;
2538 res
.mt
.ptr
[3] = mt
.ptr
[3]*ib
+m2
.mt
.ptr
[3]*blend
;
2539 res
.mt
.ptr
[4] = mt
.ptr
[4]*ib
+m2
.mt
.ptr
[4]*blend
;
2540 res
.mt
.ptr
[5] = mt
.ptr
[5]*ib
+m2
.mt
.ptr
[5]*blend
;
2541 res
.mt
.ptr
[6] = mt
.ptr
[6]*ib
+m2
.mt
.ptr
[6]*blend
;
2542 res
.mt
.ptr
[7] = mt
.ptr
[7]*ib
+m2
.mt
.ptr
[7]*blend
;
2543 res
.mt
.ptr
[8] = mt
.ptr
[8]*ib
+m2
.mt
.ptr
[8]*blend
;
2544 res
.mt
.ptr
[9] = mt
.ptr
[9]*ib
+m2
.mt
.ptr
[9]*blend
;
2545 res
.mt
.ptr
[10] = mt
.ptr
[10]*ib
+m2
.mt
.ptr
[10]*blend
;
2546 res
.mt
.ptr
[11] = mt
.ptr
[11]*ib
+m2
.mt
.ptr
[11]*blend
;
2547 res
.mt
.ptr
[12] = mt
.ptr
[12]*ib
+m2
.mt
.ptr
[12]*blend
;
2548 res
.mt
.ptr
[13] = mt
.ptr
[13]*ib
+m2
.mt
.ptr
[13]*blend
;
2549 res
.mt
.ptr
[14] = mt
.ptr
[14]*ib
+m2
.mt
.ptr
[14]*blend
;
2550 res
.mt
.ptr
[15] = mt
.ptr
[15]*ib
+m2
.mt
.ptr
[15]*blend
;
2554 Float
determinant() () const {
2555 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])-
2556 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])+
2557 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])-
2558 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]);
2561 //WARNING: this must be tested for row/col
2562 // partially ;-) taken from DarkPlaces
2563 // this assumes uniform scaling
2564 mat4
invertedSimple () const {
2565 // we only support uniform scaling, so assume the first row is enough
2566 // (note the lack of sqrt here, because we're trying to undo the scaling,
2567 // this means multiplying by the inverse scale twice - squaring it, which
2568 // makes the sqrt a waste of time)
2570 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]);
2572 mixin(ImportCoreMath
!(Float
, "sqrt"));
2573 Float scale
= cast(Float
)3/sqrt(
2574 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]+
2575 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]+
2576 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]
2583 // invert the rotation by transposing and multiplying by the squared recipricol of the input matrix scale as described above
2584 res
.mt
.ptr
[0*4+0] = mt
.ptr
[0*4+0]*scale
;
2585 res
.mt
.ptr
[1*4+0] = mt
.ptr
[0*4+1]*scale
;
2586 res
.mt
.ptr
[2*4+0] = mt
.ptr
[0*4+2]*scale
;
2587 res
.mt
.ptr
[0*4+1] = mt
.ptr
[1*4+0]*scale
;
2588 res
.mt
.ptr
[1*4+1] = mt
.ptr
[1*4+1]*scale
;
2589 res
.mt
.ptr
[2*4+1] = mt
.ptr
[1*4+2]*scale
;
2590 res
.mt
.ptr
[0*4+2] = mt
.ptr
[2*4+0]*scale
;
2591 res
.mt
.ptr
[1*4+2] = mt
.ptr
[2*4+1]*scale
;
2592 res
.mt
.ptr
[2*4+2] = mt
.ptr
[2*4+2]*scale
;
2594 // invert the translate
2595 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]);
2596 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]);
2597 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]);
2599 // don't know if there's anything worth doing here
2600 res
.mt
.ptr
[0*4+3] = cast(Float
)0;
2601 res
.mt
.ptr
[1*4+3] = cast(Float
)0;
2602 res
.mt
.ptr
[2*4+3] = cast(Float
)0;
2603 res
.mt
.ptr
[3*4+3] = cast(Float
)1;
2608 //FIXME: make this fast pasta!
2609 ref mat4
invertSimple () {
2610 mt
[] = invertedSimple().mt
[];
2614 // ////////////////////////////////////////////////////////////////////////////
2615 // compute the inverse of 4x4 Euclidean transformation matrix
2617 // Euclidean transformation is translation, rotation, and reflection.
2618 // With Euclidean transform, only the position and orientation of the object
2619 // will be changed. Euclidean transform does not change the shape of an object
2620 // (no scaling). Length and angle are reserved.
2622 // Use inverseAffine() if the matrix has scale and shear transformation.
2623 ref mat4
invertEuclidean() () {
2625 tmp
= mt
.ptr
[1]; mt
.ptr
[1] = mt
.ptr
[4]; mt
.ptr
[4] = tmp
;
2626 tmp
= mt
.ptr
[2]; mt
.ptr
[2] = mt
.ptr
[8]; mt
.ptr
[8] = tmp
;
2627 tmp
= mt
.ptr
[6]; mt
.ptr
[6] = mt
.ptr
[9]; mt
.ptr
[9] = tmp
;
2628 immutable Float x
= mt
.ptr
[12];
2629 immutable Float y
= mt
.ptr
[13];
2630 immutable Float z
= mt
.ptr
[14];
2631 mt
.ptr
[12] = -(mt
.ptr
[0]*x
+mt
.ptr
[4]*y
+mt
.ptr
[8]*z
);
2632 mt
.ptr
[13] = -(mt
.ptr
[1]*x
+mt
.ptr
[5]*y
+mt
.ptr
[9]*z
);
2633 mt
.ptr
[14] = -(mt
.ptr
[2]*x
+mt
.ptr
[6]*y
+mt
.ptr
[10]*z
);
2637 // ////////////////////////////////////////////////////////////////////////////
2638 // compute the inverse of a 4x4 affine transformation matrix
2640 // Affine transformations are generalizations of Euclidean transformations.
2641 // Affine transformation includes translation, rotation, reflection, scaling,
2642 // and shearing. Length and angle are NOT preserved.
2643 ref mat4
invertAffine() () {
2645 mixin(ImportCoreMath
!(Float
, "fabs"));
2646 // inverse 3x3 matrix
2647 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] ];
2648 r
.ptr
[0] = mt
.ptr
[0];
2649 r
.ptr
[1] = mt
.ptr
[1];
2650 r
.ptr
[2] = mt
.ptr
[2];
2651 r
.ptr
[3] = mt
.ptr
[4];
2652 r
.ptr
[4] = mt
.ptr
[5];
2653 r
.ptr
[5] = mt
.ptr
[6];
2654 r
.ptr
[6] = mt
.ptr
[8];
2655 r
.ptr
[7] = mt
.ptr
[9];
2656 r
.ptr
[8] = mt
.ptr
[10];
2658 Float
[9] tmp
= void;
2659 tmp
.ptr
[0] = r
.ptr
[4]*r
.ptr
[8]-r
.ptr
[5]*r
.ptr
[7];
2660 tmp
.ptr
[1] = r
.ptr
[2]*r
.ptr
[7]-r
.ptr
[1]*r
.ptr
[8];
2661 tmp
.ptr
[2] = r
.ptr
[1]*r
.ptr
[5]-r
.ptr
[2]*r
.ptr
[4];
2662 tmp
.ptr
[3] = r
.ptr
[5]*r
.ptr
[6]-r
.ptr
[3]*r
.ptr
[8];
2663 tmp
.ptr
[4] = r
.ptr
[0]*r
.ptr
[8]-r
.ptr
[2]*r
.ptr
[6];
2664 tmp
.ptr
[5] = r
.ptr
[2]*r
.ptr
[3]-r
.ptr
[0]*r
.ptr
[5];
2665 tmp
.ptr
[6] = r
.ptr
[3]*r
.ptr
[7]-r
.ptr
[4]*r
.ptr
[6];
2666 tmp
.ptr
[7] = r
.ptr
[1]*r
.ptr
[6]-r
.ptr
[0]*r
.ptr
[7];
2667 tmp
.ptr
[8] = r
.ptr
[0]*r
.ptr
[4]-r
.ptr
[1]*r
.ptr
[3];
2668 // check determinant if it is 0
2669 immutable Float determinant
= r
.ptr
[0]*tmp
.ptr
[0]+r
.ptr
[1]*tmp
.ptr
[3]+r
.ptr
[2]*tmp
.ptr
[6];
2670 if (fabs(determinant
) <= EPSILON
!Float
) {
2671 // cannot inverse, make it idenety matrix
2673 r
.ptr
[0] = r
.ptr
[4] = r
.ptr
[8] = 1;
2675 // divide by the determinant
2676 immutable Float invDeterminant
= cast(Float
)1/determinant
;
2677 r
.ptr
[0] = invDeterminant
*tmp
.ptr
[0];
2678 r
.ptr
[1] = invDeterminant
*tmp
.ptr
[1];
2679 r
.ptr
[2] = invDeterminant
*tmp
.ptr
[2];
2680 r
.ptr
[3] = invDeterminant
*tmp
.ptr
[3];
2681 r
.ptr
[4] = invDeterminant
*tmp
.ptr
[4];
2682 r
.ptr
[5] = invDeterminant
*tmp
.ptr
[5];
2683 r
.ptr
[6] = invDeterminant
*tmp
.ptr
[6];
2684 r
.ptr
[7] = invDeterminant
*tmp
.ptr
[7];
2685 r
.ptr
[8] = invDeterminant
*tmp
.ptr
[8];
2689 mt
.ptr
[0] = r
.ptr
[0]; mt
.ptr
[1] = r
.ptr
[1]; mt
.ptr
[2] = r
.ptr
[2];
2690 mt
.ptr
[4] = r
.ptr
[3]; mt
.ptr
[5] = r
.ptr
[4]; mt
.ptr
[6] = r
.ptr
[5];
2691 mt
.ptr
[8] = r
.ptr
[6]; mt
.ptr
[9] = r
.ptr
[7]; mt
.ptr
[10]= r
.ptr
[8];
2694 immutable Float x
= mt
.ptr
[12];
2695 immutable Float y
= mt
.ptr
[13];
2696 immutable Float z
= mt
.ptr
[14];
2697 mt
.ptr
[12] = -(r
.ptr
[0]*x
+r
.ptr
[3]*y
+r
.ptr
[6]*z
);
2698 mt
.ptr
[13] = -(r
.ptr
[1]*x
+r
.ptr
[4]*y
+r
.ptr
[7]*z
);
2699 mt
.ptr
[14] = -(r
.ptr
[2]*x
+r
.ptr
[5]*y
+r
.ptr
[8]*z
);
2701 // last row should be unchanged (0,0,0,1)
2702 //mt.ptr[3] = mt.ptr[7] = mt.ptr[11] = 0.0f;
2703 //mt.ptr[15] = 1.0f;
2708 ref mat4
invert() () {
2709 // if the 4th row is [0,0,0,1] then it is affine matrix and
2710 // it has no projective transformation
2711 if (mt
.ptr
[3] == 0 && mt
.ptr
[7] == 0 && mt
.ptr
[11] == 0 && mt
.ptr
[15] == 1) {
2712 return invertedAffine();
2714 return invertedGeneral();
2718 ///////////////////////////////////////////////////////////////////////////////
2719 // compute the inverse of a general 4x4 matrix using Cramer's Rule
2720 // if cannot find inverse, return indentity matrix
2721 ref mat4
invertGeneral() () {
2722 mixin(ImportCoreMath
!(Float
, "fabs"));
2724 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]);
2725 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]);
2726 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]);
2727 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]);
2729 immutable Float determinant
= mt
.ptr
[0]*cofactor0
-mt
.ptr
[1]*cofactor1
+mt
.ptr
[2]*cofactor2
-mt
.ptr
[3]*cofactor3
;
2730 if (fabs(determinant
) <= EPSILON
!Float
) { this = Identity
; return this; }
2732 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]);
2733 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]);
2734 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]);
2735 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]);
2737 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]);
2738 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]);
2739 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]);
2740 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]);
2742 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]);
2743 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]);
2744 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]);
2745 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]);
2747 immutable Float invDeterminant
= cast(Float
)1/determinant
;
2748 mt
.ptr
[0] = invDeterminant
*cofactor0
;
2749 mt
.ptr
[1] = -invDeterminant
*cofactor4
;
2750 mt
.ptr
[2] = invDeterminant
*cofactor8
;
2751 mt
.ptr
[3] = -invDeterminant
*cofactor12
;
2753 mt
.ptr
[4] = -invDeterminant
*cofactor1
;
2754 mt
.ptr
[5] = invDeterminant
*cofactor5
;
2755 mt
.ptr
[6] = -invDeterminant
*cofactor9
;
2756 mt
.ptr
[7] = invDeterminant
*cofactor13
;
2758 mt
.ptr
[8] = invDeterminant
*cofactor2
;
2759 mt
.ptr
[9] = -invDeterminant
*cofactor6
;
2760 mt
.ptr
[10]= invDeterminant
*cofactor10
;
2761 mt
.ptr
[11]= -invDeterminant
*cofactor14
;
2763 mt
.ptr
[12]= -invDeterminant
*cofactor3
;
2764 mt
.ptr
[13]= invDeterminant
*cofactor7
;
2765 mt
.ptr
[14]= -invDeterminant
*cofactor11
;
2766 mt
.ptr
[15]= invDeterminant
*cofactor15
;
2771 // compute cofactor of 3x3 minor matrix without sign
2772 // input params are 9 elements of the minor matrix
2773 // NOTE: The caller must know its sign
2774 private static Float
getCofactor() (Float m0
, Float m1
, Float m2
, Float m3
, Float m4
, Float m5
, Float m6
, Float m7
, Float m8
) {
2775 pragma(inline
, true);
2776 return m0
*(m4
*m8
-m5
*m7
)-m1
*(m3
*m8
-m5
*m6
)+m2
*(m3
*m7
-m4
*m6
);
2779 Quat4
!VT
toQuaternion () const nothrow @trusted @nogc {
2780 mixin(ImportCoreMath
!(Float
, "sqrt"));
2781 Quat4
!VT res
= void;
2782 immutable Float tr
= mt
.ptr
[0*4+0]+mt
.ptr
[1*4+1]+mt
.ptr
[2*4+2];
2783 // check the diagonal
2785 Float s
= sqrt(tr
+cast(Float
)1);
2786 res
.w
= s
/cast(Float
)2;
2787 s
= cast(Float
)0.5/s
;
2788 res
.x
= (mt
.ptr
[2*4+1]-mt
.ptr
[1*4+2])*s
;
2789 res
.y
= (mt
.ptr
[0*4+2]-mt
.ptr
[2*4+0])*s
;
2790 res
.z
= (mt
.ptr
[1*4+0]-mt
.ptr
[0*4+1])*s
;
2792 // diagonal is negative
2793 int[3] nxt
= [1, 2, 0];
2795 if (mt
.ptr
[1*4+1] > mt
.ptr
[0*4+0]) i
= 1;
2796 if (mt
.ptr
[2*4+2] > mt
.ptr
[i
*4+i
]) i
= 2;
2799 Float s
= sqrt((mt
.ptr
[i
*4+i
]-(mt
.ptr
[j
*4+j
]+mt
.ptr
[k
*4+k
]))+cast(Float
)1);
2801 q
.ptr
[i
] = s
*cast(Float
)0.5;
2802 if (s
!= 0) s
= cast(Float
)0.5/s
;
2803 q
.ptr
[3] = (mt
.ptr
[k
*4+j
]-mt
.ptr
[j
*4+k
])*s
;
2804 q
.ptr
[j
] = (mt
.ptr
[j
*4+i
]+mt
.ptr
[i
*4+j
])*s
;
2805 q
.ptr
[k
] = (mt
.ptr
[k
*4+i
]+mt
.ptr
[i
*4+k
])*s
;
2816 // ////////////////////////////////////////////////////////////////////////// //
2817 alias quat4
= Quat4
!vec3
;
2819 align(1) struct Quat4(VT
) if (IsVector
!VT
) {
2822 alias Float
= VT
.Float
;
2823 alias quat4
= typeof(this);
2826 Float w
=1, x
=0, y
=0, z
=0; // default: identity
2829 this (Float aw
, Float ax
, Float ay
, Float az
) nothrow @trusted @nogc {
2830 pragma(inline
, true);
2837 // valid only for unit quaternions
2838 this (Float ax
, Float ay
, Float az
) nothrow @trusted @nogc {
2839 immutable Float t
= cast(Float
)1-(ax
*ax
)-(ay
*ay
)-(az
*az
);
2843 mixin(ImportCoreMath
!(Float
, "sqrt"));
2851 this() (in auto ref VT v
) nothrow @safe @nogc {
2852 pragma(inline
, true);
2859 // the rotation of a point by a quaternion is given by the formula:
2861 // where R is the resultant quaternion, Q is the orientation quaternion by which you want to perform a rotation,
2862 // Q* the conjugate of Q and P is the point converted to a quaternion.
2863 // note: here the "." is the multiplication operator.
2865 // to convert a 3D vector to a quaternion, copy the x, y and z components and set the w component to 0.
2866 // this is the same for quaternion to vector conversion: take the x, y and z components and forget the w.
2868 VT
asVector () const nothrow @safe @nogc { pragma(inline
, true); return VT(x
, y
, z
); }
2870 static quat4
Identity () nothrow @safe @nogc { pragma(inline
, true); return quat4(1, 0, 0, 0); }
2871 bool isIdentity () const nothrow @safe @nogc { pragma(inline
, true); return (w
== 1 && x
== 0 && y
== 0 && z
== 0); }
2873 static quat4
fromAngles (Float roll
, Float pitch
, Float yaw
) nothrow @trusted @nogc {
2874 mixin(ImportCoreMath
!(Float
, "cos", "sin"));
2875 // calculate trig identities
2876 immutable Float cr
= cos(roll
/2);
2877 immutable Float cp
= cos(pitch
/2);
2878 immutable Float cy
= cos(yaw
/2);
2879 immutable Float sr
= sin(roll
/2);
2880 immutable Float sp
= sin(pitch
/2);
2881 immutable Float sy
= sin(yaw
/2);
2882 immutable Float cpcy
= cp
*cy
;
2883 immutable Float spsy
= sp
*sy
;
2892 //@property bool valid () const nothrow @safe @nogc { pragma(inline, true); import core.stdc.math : isnan; return !isnan(w) && !isnan(x) && !isnan(y) && !isnan(z); }
2893 @property bool valid () const nothrow @safe @nogc { pragma(inline
, true); import core
.stdc
.math
: isfinite
; return isfinite(w
) && isfinite(x
) && isfinite(y
) && isfinite(z
); }
2896 Float
opIndex (usize idx
) const nothrow @safe @nogc {
2897 pragma(inline
, true);
2907 void opIndexAssign (Float v
, usize idx
) {
2908 pragma(inline
, true);
2909 if (idx
== 0) x
= v
; else
2910 if (idx
== 1) y
= v
; else
2911 if (idx
== 2) z
= v
; else
2912 if (idx
== 3) w
= v
; else
2913 assert(0, "invalid quaternion index");
2916 Mat4
!VT
toMatrix () const nothrow @trusted @nogc {
2917 // calculate coefficients
2918 immutable Float x2
= this.x
+this.x
;
2919 immutable Float y2
= this.y
+this.y
;
2920 immutable Float z2
= this.z
+this.z
;
2921 immutable Float xx
= this.x
*x2
;
2922 immutable Float xy
= this.x
*y2
;
2923 immutable Float xz
= this.x
*z2
;
2924 immutable Float yy
= this.y
*y2
;
2925 immutable Float yz
= this.y
*z2
;
2926 immutable Float zz
= this.z
*z2
;
2927 immutable Float wx
= this.w
*x2
;
2928 immutable Float wy
= this.w
*y2
;
2929 immutable Float wz
= this.w
*z2
;
2932 res
.mt
.ptr
[0*4+0] = cast(Float
)1-(yy
+zz
);
2933 res
.mt
.ptr
[0*4+1] = xy
-wz
;
2934 res
.mt
.ptr
[0*4+2] = xz
+wy
;
2935 res
.mt
.ptr
[0*4+3] = 0;
2937 res
.mt
.ptr
[1*4+0] = xy
+wz
;
2938 res
.mt
.ptr
[1*4+1] = cast(Float
)1-(xx
+zz
);
2939 res
.mt
.ptr
[1*4+2] = yz
-wx
;
2940 res
.mt
.ptr
[1*4+3] = 0;
2942 res
.mt
.ptr
[2*4+0] = xz
-wy
;
2943 res
.mt
.ptr
[2*4+1] = yz
+wx
;
2944 res
.mt
.ptr
[2*4+2] = cast(Float
)1-(xx
+yy
);
2945 res
.mt
.ptr
[2*4+3] = 0;
2947 res
.mt
.ptr
[3*4+0] = 0;
2948 res
.mt
.ptr
[3*4+1] = 0;
2949 res
.mt
.ptr
[3*4+2] = 0;
2950 res
.mt
.ptr
[3*4+3] = 1;
2955 auto opUnary(string op
:"+") () const nothrow @safe @nogc { pragma(inline
, true); return this; }
2957 // for unit quaternions, this is inverse/conjugate
2958 auto opUnary(string op
:"-") () const nothrow @safe @nogc { pragma(inline
, true); return quat4(-w
, -x
, -y
, -z
); }
2960 quat4
opBinary(string op
:"*") (in auto ref quat4 q2
) const nothrow @safe @nogc {
2961 auto res
= quat4(this.w
, this.x
, this.y
, this.z
);
2965 ref quat4
opOpAssign(string op
:"*") (in auto ref quat4 q2
) nothrow @safe @nogc {
2966 immutable Float A
= (this.w
+this.x
)*(q2
.w
+q2
.x
);
2967 immutable Float B
= (this.z
-this.y
)*(q2
.y
-q2
.z
);
2968 immutable Float C
= (this.w
-this.x
)*(q2
.y
+q2
.z
);
2969 immutable Float D
= (this.y
+this.z
)*(q2
.w
-q2
.x
);
2970 immutable Float E
= (this.x
+this.z
)*(q2
.x
+q2
.y
);
2971 immutable Float F
= (this.x
-this.z
)*(q2
.x
-q2
.y
);
2972 immutable Float G
= (this.w
+this.y
)*(q2
.w
-q2
.z
);
2973 immutable Float H
= (this.w
-this.y
)*(q2
.w
+q2
.z
);
2974 this.w
= B
+(-E
-F
+G
+H
)/2;
2975 this.x
= A
-(E
+F
+G
+H
)/2;
2976 this.y
= C
+(E
-F
+G
-H
)/2;
2977 this.z
= D
+(E
-F
-G
+H
)/2;
2981 quat4
slerp() (in auto ref quat4 to
, Float t
) const nothrow @trusted @nogc {
2982 mixin(ImportCoreMath
!(Float
, "acos", "sin"));
2983 Float
[4] to1
= void;
2985 Float cosom
= this.x
*to
.x
+this.y
*to
.y
+this.z
*to
.z
+this.w
*to
.w
;
2986 // adjust signs (if necessary)
2999 Float scale0
= void, scale1
= void;
3000 // calculate coefficients
3001 if (cast(Float
)1-cosom
> EPSILON
!Float
) {
3002 // standard case (slerp)
3003 immutable Float omega
= acos(cosom
);
3004 immutable Float sinom
= sin(omega
);
3005 scale0
= sin((cast(Float
)1-t
)*omega
)/sinom
;
3006 scale1
= sin(t
*omega
)/sinom
;
3008 // "from" and "to" quaternions are very close, so we can do a linear interpolation
3009 scale0
= cast(Float
)1-t
;
3012 // calculate final values
3014 scale0
*this.w
+scale1
*to1
.ptr
[3],
3015 scale0
*this.x
+scale1
*to1
.ptr
[0],
3016 scale0
*this.y
+scale1
*to1
.ptr
[1],
3017 scale0
*this.z
+scale1
*to1
.ptr
[2],
3023 // ////////////////////////////////////////////////////////////////////////// //
3024 alias OBB2D
= OBB2d
!vec2
;
3026 /// Oriented Bounding Box
3027 struct OBB2d(VT
) if (IsVectorDim
!(VT
, 2)) {
3028 // to make the tests extremely efficient, `origin` stores the
3029 // projection of corner number zero onto a box's axes and the axes are stored
3030 // explicitly in axis. the magnitude of these stored axes is the inverse
3031 // of the corresponding edge length so that all overlap tests can be performed on
3032 // the interval [0, 1] without normalization, and square roots are avoided
3033 // throughout the entire test.
3035 alias Me
= typeof(this);
3036 alias Float
= VT
.Float
;
3040 VT
[4] corner
; // corners of the box, where 0 is the lower left
3041 bool aovalid
; // are axes and origin valid?
3042 VT
[2] axis
; // two edges of the box extended away from corner[0]
3043 VT
.Float
[2] origin
; // origin[a] = corner[0].dot(axis[a]);
3045 private nothrow @trusted @nogc:
3046 // returns true if other overlaps one dimension of this
3047 bool overlaps1Way() (in auto ref Me other
) const {
3048 foreach (immutable a
; 0..2) {
3049 Float t
= other
.corner
.ptr
[0].dot(axis
.ptr
[a
]);
3050 // find the extent of box 2 on axis a
3051 Float tMin
= t
, tMax
= t
;
3052 foreach (immutable c
; 1..4) {
3053 t
= other
.corner
.ptr
[c
].dot(axis
.ptr
[a
]);
3054 if (t
< tMin
) tMin
= t
; else if (t
> tMax
) tMax
= t
;
3056 // we have to subtract off the origin
3057 // see if [tMin, tMax] intersects [0, 1]
3058 if (tMin
> 1+origin
.ptr
[a
] || tMax
< origin
.ptr
[a
]) {
3059 // there was no intersection along this dimension; the boxes cannot possibly overlap
3063 // there was no dimension along which there is no intersection: therefore the boxes overlap
3067 // updates the axes after the corners move; assumes the corners actually form a rectangle
3068 void computeAxes () {
3069 axis
.ptr
[0] = corner
.ptr
[1]-corner
.ptr
[0];
3070 axis
.ptr
[1] = corner
.ptr
[3]-corner
.ptr
[0];
3071 // 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
3072 foreach (immutable a
; 0..2) {
3073 axis
.ptr
[a
] /= axis
.ptr
[a
].lengthSquared
;
3074 origin
.ptr
[a
] = corner
.ptr
[0].dot(axis
.ptr
[a
]);
3081 this() (in auto ref VT center
, in Float w
, in Float h
, in Float angle
) {
3082 mixin(ImportCoreMath
!(Float
, "cos", "sin"));
3084 immutable Float ca
= cos(angle
);
3085 immutable Float sa
= sin(angle
);
3086 auto ox
= VT( ca
, sa
);
3087 auto oy
= VT(-sa
, ca
);
3092 corner
.ptr
[0] = center
-ox
-oy
;
3093 corner
.ptr
[1] = center
+ox
-oy
;
3094 corner
.ptr
[2] = center
+ox
+oy
;
3095 corner
.ptr
[3] = center
-ox
+oy
;
3097 // compute axes on demand
3101 VT
[4] corners () const { pragma(inline
, true); VT
[4] res
= corner
[]; return res
; }
3104 VT
opIndex (usize idx
) const {
3105 pragma(inline
, true);
3106 return (idx
< 4 ? corner
.ptr
[idx
] : VT(Float
.nan
, Float
.nan
, Float
.nan
));
3110 void moveTo() (in auto ref VT center
) {
3111 immutable centroid
= (corner
.ptr
[0]+corner
.ptr
[1]+corner
.ptr
[2]+corner
.ptr
[3])/4;
3112 immutable translation
= center
-centroid
;
3113 foreach (ref VT cv
; corner
[]) cv
+= translation
;
3114 aovalid
= false; // invalidate axes
3118 void moveBy() (in auto ref VT delta
) {
3119 foreach (ref VT cv
; corner
[]) cv
+= delta
;
3120 aovalid
= false; // invalidate axes
3123 /// rotate around centroid
3124 void rotate() (Float angle
) {
3125 mixin(ImportCoreMath
!(Float
, "cos", "sin"));
3126 immutable centroid
= (corner
.ptr
[0]+corner
.ptr
[1]+corner
.ptr
[2]+corner
.ptr
[3])/4;
3127 immutable Float ca
= cos(angle
);
3128 immutable Float sa
= sin(angle
);
3129 foreach (ref cv
; corner
[]) {
3130 immutable Float ox
= cv
.x
-centroid
.x
;
3131 immutable Float oy
= cv
.y
-centroid
.y
;
3132 cv
.x
= ox
*ca
-oy
*sa
+centroid
.x
;
3133 cv
.y
= ox
*sa
+oy
*ca
+centroid
.y
;
3135 aovalid
= false; // invalidate axes
3139 void scale() (Float mult
) {
3140 immutable centroid
= (corner
.ptr
[0]+corner
.ptr
[1]+corner
.ptr
[2]+corner
.ptr
[3])/4;
3141 foreach (ref cv
; corner
[]) cv
= (cv
-centroid
)*mult
+centroid
;
3142 aovalid
= false; // invalidate axes
3145 /// apply transformation matrix, assuming that world (0,0) is at centroid
3146 void transform() (in auto ref Mat3
!VT mat
) {
3147 immutable centroid
= (corner
.ptr
[0]+corner
.ptr
[1]+corner
.ptr
[2]+corner
.ptr
[3])/4;
3148 foreach (ref VT cv
; corner
[]) cv
-= centroid
;
3149 foreach (ref VT cv
; corner
[]) cv
= cv
*mat
;
3150 foreach (ref VT cv
; corner
[]) cv
+= centroid
;
3151 aovalid
= false; // invalidate axes
3154 /// returns true if the intersection of the boxes is non-empty
3155 bool overlaps() (in auto ref Me other
) {
3156 pragma(inline
, true);
3157 if (!aovalid
) computeAxes(); // fix axes if necessary
3158 return (overlaps1Way(other
) && other
.overlaps1Way(this));
3163 // ////////////////////////////////////////////////////////////////////////// //
3164 template IsSphere(ST
) {
3165 static if (is(ST
== Sphere
!SVT
, SVT
)) {
3166 enum IsSphere
= IsVector
!SVT
;
3168 enum IsSphere
= false;
3172 template IsSphere(ST
, VT
) {
3173 static if (is(ST
== Sphere
!SVT
, SVT
)) {
3174 enum IsSphere
= (is(VT
== SVT
) && IsVector
!SVT
);
3176 enum IsSphere
= false;
3182 struct Sphere(VT
) if (IsVector
!VT
) {
3184 alias Float
= VT
.Float
;
3186 alias Me
= typeof(this);
3189 vec orig
; /// sphere origin
3190 Float radius
; /// sphere radius
3192 public nothrow @trusted @nogc:
3193 this() (in auto ref vec aorig
, Float arad
) { orig
= aorig
; radius
= arad
; }
3195 @property bool valid () const { import core
.stdc
.math
: isfinite
; pragma(inline
, true); return (isfinite(radius
) && radius
> 0); }
3198 bool sweep() (in auto ref vec amove
, in auto ref Me sb
, Float
* u0
, Float
* u1
) const {
3199 mixin(ImportCoreMath
!(Float
, "sqrt"));
3201 immutable odist
= sb
.orig
-this.orig
; // vector from A0 to B0
3202 immutable vab
= -amove
; // relative velocity (in normalized time)
3203 immutable rab
= this.radius
+sb
.radius
;
3204 immutable Float a
= vab
.dot(vab
); // u*u coefficient
3205 immutable Float b
= cast(Float
)2*vab
.dot(odist
); // u coefficient
3206 immutable Float c
= odist
.dot(odist
)-rab
*rab
; // constant term
3208 // check if they're currently overlapping
3209 if (odist
.dot(odist
) <= rab
*rab
) {
3210 if (u0
!is null) *u0
= 0;
3211 if (u0
!is null) *u1
= 0;
3215 // check if they hit each other during the frame
3216 immutable Float q
= b
*b
-4*a
*c
;
3217 if (q
< 0) return false; // alas, complex roots
3219 immutable Float sq
= sqrt(q
);
3220 immutable Float d
= cast(Float
)1/(cast(Float
)2*a
);
3221 Float uu0
= (-b
+sq
)*d
;
3222 Float uu1
= (-b
-sq
)*d
;
3224 if (uu0
> uu1
) { immutable t
= uu0
; uu0
= uu1
; uu1
= t
; } // swap
3225 if (u0
!is null) *u0
= uu0
;
3226 if (u1
!is null) *u1
= uu1
;
3231 // sweep test; if `true` (hit), `hitpos` will be sphere position when it hits this plane, and `u` will be normalized collision time
3232 bool sweep(PT
) (in auto ref PT plane
, in auto ref vec3 amove
, vec3
* hitpos
, Float
* u
) const if (IsPlane3
!(PT
, Float
)) {
3233 pragma(inline
, true);
3234 return plane
.sweep(this, amove
, hitpos
, u
);
3239 // ////////////////////////////////////////////////////////////////////////// //
3240 template IsPlane3(PT
) {
3241 static if (is(PT
== Plane3
!(PFT
, eps
, swiz
), PFT
, double eps
, bool swiz
)) {
3242 enum IsPlane3
= (is(PFT
== float) ||
is(PFT
== double));
3244 enum IsPlane3
= false;
3249 template IsPlane3(PT
, FT
) {
3250 static if (is(PT
== Plane3
!(PFT
, eps
, swiz
), PFT
, double eps
, bool swiz
)) {
3251 enum IsPlane3
= (is(FT
== PFT
) && (is(PFT
== float) ||
is(PFT
== double)));
3253 enum IsPlane3
= false;
3258 // plane in 3D space: Ax+By+Cz+D=0
3259 align(1) struct Plane3(FloatType
=VFloat
, double PlaneEps
=-1.0, bool enableSwizzling
=true) if (is(FloatType
== float) ||
is(FloatType
== double)) {
3262 alias Float
= FloatType
;
3263 alias plane3
= typeof(this);
3264 alias Me
= typeof(this);
3265 alias vec3
= VecN
!(3, Float
);
3266 static if (PlaneEps
< 0) {
3267 enum EPS
= EPSILON
!Float
;
3269 enum EPS
= cast(Float
)PlaneEps
;
3273 alias PType
= ubyte;
3282 //Float a = 0, b = 0, c = 0, d = 0;
3287 string
toString () const {
3288 import std
.string
: format
;
3290 return "(%s,%s,%s,%s)".format(normal
.x
, normal
.y
, normal
.z
, w
);
3291 } catch (Exception
) {
3297 this() (in auto ref vec3 aorigin
, in auto ref vec3 anormal
) { pragma(inline
, true); setOriginNormal(aorigin
, anormal
); }
3298 this() (in auto ref vec3 anormal
, Float aw
) { pragma(inline
, true); set(anormal
, aw
); }
3299 this() (in auto ref vec3 a
, in auto ref vec3 b
, in auto ref vec3 c
) { pragma(inline
, true); setFromPoints(a
, b
, c
); }
3301 @property Float
offset () const pure { pragma(inline
, true); return -w
; }
3303 void set () (in auto ref vec3 anormal
, Float aw
) {
3304 pragma(inline
, true);
3309 void setOriginNormal () (in auto ref vec3 aorigin
, in auto ref vec3 anormal
) {
3310 normal
= anormal
.normalized
;
3312 w
= normal
.x
*aorigin
.x
+normal
.y
*aorigin
.y
+normal
.z
*aorigin
.z
;
3315 void setFromPoints() (in auto ref vec3 a
, in auto ref vec3 b
, in auto ref vec3 c
) @trusted {
3316 //normal = ((b-a)^(c-a)).normalized;
3317 immutable vec3 b0
= b
-a
;
3318 immutable vec3 c0
= c
-a
;
3319 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
);
3323 //w = normal*a; // n.dot(a)
3324 w
= normal
.x
*a
.x
+normal
.y
*a
.y
+normal
.z
*a
.z
;
3326 //normalize; // just in case, to tolerate some floating point errors
3329 immutable double len
= normal
.dbllength
;
3330 //{ import core.stdc.stdio; printf("**len=%g\n", len); }
3331 if (len
<= EPSILON
!Float
) {
3333 //{ import core.stdc.stdio; printf(" OOPS: n=(%g,%g,%g)\n", cast(double)normal.x, cast(double)normal.y, cast(double)normal.z); }
3334 normal
= vec3
.Invalid
;
3338 version(vmath_slow_normalize
) {
3343 immutable double dd = 1.0/len
;
3348 w
= normal
.x
*a
.x
+normal
.y
*a
.y
+normal
.z
*a
.z
;
3351 normalize; // just in case, to tolerate some floating point errors
3359 @property bool valid () const { pragma(inline
, true); import core
.stdc
.math
: isfinite
; return (isfinite(w
) != 0); }
3361 Float
opIndex (usize idx
) const {
3362 pragma(inline
, true);
3363 return (idx
== 0 ? normal
.x
: idx
== 1 ? normal
.y
: idx
== 2 ? normal
.z
: idx
== 3 ? w
: Float
.nan
);
3366 void opIndexAssign (Float v
, usize idx
) {
3367 pragma(inline
, true);
3368 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
;
3371 ref plane3
normalize () {
3372 if (!normal
.isFinite
) {
3373 normal
= vec3
.Invalid
;
3378 mixin(ImportCoreMath
!(Float
, "fabs"));
3379 double dd = normal
.dbllength
;
3380 if (fabs(1.0-dd) > EPSILON
!Float
) {
3381 version(vmath_slow_normalize
) {
3387 dd = cast(Float
)1/dd;
3395 mixin(ImportCoreMath
!(double, "sqrt"));
3396 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
);
3397 version(vmath_slow_normalize
) {
3413 //WARNING! won't check if this plane is valid
3415 pragma(inline
, true);
3420 //WARNING! won't check if this plane is valid
3421 plane3
fliped () const {
3422 pragma(inline
, true);
3423 return plane3(-normal
, -w
);
3426 PType
pointSide() (in auto ref vec3 p
) const {
3427 pragma(inline
, true);
3428 //immutable Float t = (normal*p)-w; // dot
3429 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
;
3430 return (t
< -EPS ? Back
: (t
> EPS ? Front
: Coplanar
));
3433 double pointSideD() (in auto ref vec3 p
) const {
3434 pragma(inline
, true);
3435 //return (normal*p)-w; // dot
3436 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
;
3439 Float
pointSideF() (in auto ref vec3 p
) const {
3440 pragma(inline
, true);
3441 //return (normal*p)-w; // dot
3442 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
);
3445 // distance from point to plane
3446 // plane must be normalized
3447 double dbldistance() (in auto ref vec3 p
) const {
3448 pragma(inline
, true);
3449 return (cast(double)normal
.x
*p
.x
+cast(double)normal
.y
*p
.y
+cast(double)normal
.z
*cast(double)p
.z
)/normal
.dbllength
;
3452 // distance from point to plane
3453 // plane must be normalized
3454 Float
distance() (in auto ref vec3 p
) const {
3455 pragma(inline
, true);
3456 return cast(Float
)dbldistance
;
3459 // "land" point onto the plane
3460 // plane must be normalized
3461 vec3
landAlongNormal() (in auto ref vec3 p
) const {
3462 pragma(inline
, true);
3463 mixin(ImportCoreMath
!(Float
, "fabs"));
3464 immutable double pdist
= pointSideF(p
);
3465 return (fabs(pdist
) > EPSILON
!Float ? p
-normal
*pdist
: p
);
3468 // "land" point onto the plane
3469 // plane must be normalized
3471 vec3 land() (in auto ref vec3 p) const {
3472 mixin(ImportCoreMath!(double, "fabs"));
3473 // distance from point to plane
3474 double pdist = (cast(double)normal.x*p.x+cast(double)normal.y*p.y+cast(double)normal.z*cast(double)p.z)/normal.dbllength;
3475 // just-in-case check
3476 if (fabs(pdist) > EPSILON!double) {
3478 return p+normal*pdist;
3486 // plane must be normalized
3487 vec3
project() (in auto ref vec3 v
) const {
3488 pragma(inline
, true);
3489 mixin(ImportCoreMath
!(double, "fabs"));
3490 return v
-(v
-normal
*w
).dot(normal
)*normal
;
3493 // returns the point where the line p0-p1 intersects this plane
3494 vec3
lineIntersect() (in auto ref vec3 p0
, in auto ref vec3 p1
) const {
3495 pragma(inline
, true);
3496 immutable dif
= p1
-p0
;
3497 immutable t
= (w
-normal
.dot(p0
))/normal
.dot(dif
);
3502 static if (enableSwizzling
) auto opDispatch(string
fld) () if (isGoodSwizzling
!(fld, "xyzw", 2, 3)) {
3503 static if (fld.length
== 2) {
3504 return mixin(SwizzleCtor
!("vec2", fld));
3506 return mixin(SwizzleCtor
!("vec3", fld));
3510 // sweep test; if `true` (hit), `hitpos` will be sphere position when it hits this plane, and `u` will be normalized collision time
3511 bool sweep(ST
) (in auto ref ST sphere
, in auto ref vec3 amove
, vec3
* hitpos
, Float
* u
) const if (IsSphere
!(ST
, vec3
)) {
3512 mixin(ImportCoreMath
!(Float
, "fabs"));
3513 immutable c0
= sphere
.orig
;
3514 immutable c1
= c0
+amove
;
3515 immutable Float r
= sphere
.radius
;
3516 immutable Float d0
= (normal
*c0
)+w
;
3517 immutable Float d1
= (normal
*c1
)+w
;
3518 // check if the sphere is touching the plane
3519 if (fabs(d0
) <= r
) {
3520 if (hitpos
!is null) *hitpos
= c0
;
3521 if (u
!is null) *u
= 0;
3524 // check if the sphere penetrated during movement
3525 if (d0
> r
&& d1
< r
) {
3526 immutable Float uu
= (d0
-r
)/(d0
-d1
); // normalized time
3527 if (u
!is null) *u
= uu
;
3528 if (hitpos
!is null) *hitpos
= (1-uu
)*c0
+uu
*c1
; // point of first contact
3535 // intersection of 3 planes, Graphics Gems 1 pg 305
3536 auto intersectionPoint() (in auto ref plane3 plane2
, in auto ref plane3 plane3
) const {
3537 mixin(ImportCoreMath
!(Float
, "fabs"));
3538 alias plane1
= this;
3539 immutable Float det
= plane1
.normal
.cross(plane2
.normal
).dot(plane3
.normal
);
3540 // if the determinant is 0, that means parallel planes, no intersection
3541 if (fabs(det
) < EPSILON
!Float
) return vec3
.Invalid
;
3543 (plane2
.normal
.cross(plane3
.normal
)*(-plane1
.w
)+
3544 plane3
.normal
.cross(plane1
.normal
)*(-plane2
.w
)+
3545 plane1
.normal
.cross(plane2
.normal
)*(-plane3
.w
))/det
;
3550 // ////////////////////////////////////////////////////////////////////////// //
3551 struct Ray(VT
) if (IsVector
!VT
) {
3554 alias Float
= VT
.Float
;
3557 VT orig
, dir
; // dir should be normalized (setters does this)
3560 string
toString () const {
3561 import std
.format
: format
;
3563 return "[(%s,%s):(%s,%s)]".format(orig
.x
, orig
.y
, dir
.x
, dir
.y
);
3564 } catch (Exception
) {
3570 this (VT
.Float x
, VT
.Float y
, VT
.Float angle
) { pragma(inline
, true); setOrigDir(x
, y
, angle
); }
3571 this() (in auto ref VT aorg
, VT
.Float angle
) { pragma(inline
, true); setOrigDir(aorg
, angle
); }
3573 static Ray
!VT
fromPoints() (in auto ref VT p0
, in auto ref VT p1
) {
3574 pragma(inline
, true);
3577 res
.dir
= (p1
-p0
).normalized
;
3581 static if (VT
.Dims
== 2) void setOrigDir (VT
.Float x
, VT
.Float y
, VT
.Float angle
) {
3582 pragma(inline
, true);
3583 mixin(ImportCoreMath
!(Float
, "cos", "sin"));
3590 static if (VT
.Dims
== 2) void setOrigDir() (in auto ref VT aorg
, VT
.Float angle
) {
3591 pragma(inline
, true);
3592 mixin(ImportCoreMath
!(Float
, "cos", "sin"));
3599 static if (VT
.Dims
== 2) void setOrig (VT
.Float x
, VT
.Float y
) {
3600 pragma(inline
, true);
3605 void setOrig() (in auto ref VT aorg
) {
3606 pragma(inline
, true);
3609 static if (VT
.Dims
== 3) orig
.z
= aorg
.z
;
3612 static if (VT
.Dims
== 2) void setDir (VT
.Float angle
) {
3613 pragma(inline
, true);
3614 mixin(ImportCoreMath
!(Float
, "cos", "sin"));
3619 @property VT
right () const {
3620 pragma(inline
, true);
3621 static if (VT
.Dims
== 2) return VT(dir
.y
, -dir
.x
);
3622 else return VT(dir
.y
, -dir
.x
, dir
.z
);
3625 VT
pointAt (VT
.Float len
) const { pragma(inline
, true); return orig
+dir
*len
; }
3629 // ////////////////////////////////////////////////////////////////////////// //
3630 public struct AABBImpl(VT
) if (IsVector
!VT
) {
3632 static T
nmin(T
) (in T a
, in T b
) { pragma(inline
, true); return (a
< b ? a
: b
); }
3633 static T
nmax(T
) (in T a
, in T b
) { pragma(inline
, true); return (a
> b ? a
: b
); }
3637 alias Float
= VT
.Float
;
3638 alias Me
= typeof(this);
3642 //VT half; // should be positive
3646 string
toString () const {
3647 import std
.format
: format
;
3648 return "[%s-%s]".format(min
, max
);
3651 public nothrow @safe @nogc:
3652 this() (in auto ref VT amin
, in auto ref VT amax
) {
3653 pragma(inline
, true);
3654 //center = (amin+amax)/2;
3655 //half = VT((nmax(amin.x, amax.x)-nmin(amin.x, amax.x))/2, (nmax(amin.y, amax.y)-nmin(amin.y, amax.y))/2);
3657 // this breaks VecN ctor inliner (fuck!)
3658 static if (VT
.Dims
== 2) {
3659 min
= VT(nmin(amin
.x
, amax
.x
), nmin(amin
.y
, amax
.y
));
3660 max
= VT(nmax(amin
.x
, amax
.x
), nmax(amin
.y
, amax
.y
));
3662 min
= VT(nmin(amin
.x
, amax
.x
), nmin(amin
.y
, amax
.y
), nmin(amin
.z
, amax
.z
));
3663 max
= VT(nmax(amin
.x
, amax
.x
), nmax(amin
.y
, amax
.y
), nmax(amin
.z
, amax
.z
));
3666 min
.x
= nmin(amin
.x
, amax
.x
);
3667 min
.y
= nmin(amin
.y
, amax
.y
);
3668 static if (VT
.Dims
== 3) min
.z
= nmin(amin
.z
, amax
.z
);
3669 max
.x
= nmax(amin
.x
, amax
.x
);
3670 max
.y
= nmax(amin
.y
, amax
.y
);
3671 static if (VT
.Dims
== 3) max
.z
= nmax(amin
.z
, amax
.z
);
3676 static if (VT
.Dims
== 2) {
3677 min
= VT(+VT
.Float
.infinity
, +VT
.Float
.infinity
);
3678 max
= VT(-VT
.Float
.infinity
, -VT
.Float
.infinity
);
3680 min
= VT(+VT
.Float
.infinity
, +VT
.Float
.infinity
, +VT
.Float
.infinity
);
3681 max
= VT(-VT
.Float
.infinity
, -VT
.Float
.infinity
, -VT
.Float
.infinity
);
3685 void setMinMax() (in auto ref VT amin
, in auto ref VT amax
) pure {
3686 pragma(inline
, true);
3688 // this breaks VecN ctor inliner (fuck!)
3689 static if (VT
.Dims
== 2) {
3690 min
= VT(nmin(amin
.x
, amax
.x
), nmin(amin
.y
, amax
.y
));
3691 max
= VT(nmax(amin
.x
, amax
.x
), nmax(amin
.y
, amax
.y
));
3693 min
= VT(nmin(amin
.x
, amax
.x
), nmin(amin
.y
, amax
.y
), nmin(amin
.z
, amax
.z
));
3694 max
= VT(nmax(amin
.x
, amax
.x
), nmax(amin
.y
, amax
.y
), nmax(amin
.z
, amax
.z
));
3697 min
.x
= nmin(amin
.x
, amax
.x
);
3698 min
.y
= nmin(amin
.y
, amax
.y
);
3699 static if (VT
.Dims
== 3) min
.z
= nmin(amin
.z
, amax
.z
);
3700 max
.x
= nmax(amin
.x
, amax
.x
);
3701 max
.y
= nmax(amin
.y
, amax
.y
);
3702 static if (VT
.Dims
== 3) max
.z
= nmax(amin
.z
, amax
.z
);
3706 //@property VT min () const pure { pragma(inline, true); return center-half; }
3707 //@property VT max () const pure { pragma(inline, true); return center+half; }
3709 VT
center () const pure { pragma(inline
, true); return (min
+max
)/2; }
3710 VT
extent () const pure { pragma(inline
, true); return max
-min
; }
3712 //@property valid () const { pragma(inline, true); return center.isFinite && half.isFinite; }
3713 @property valid () const {
3714 pragma(inline
, true);
3715 static if (VT
.Dims
== 2) {
3716 return (min
.isFinite
&& max
.isFinite
&& min
.x
<= max
.x
&& min
.y
<= max
.y
);
3718 return (min
.isFinite
&& max
.isFinite
&& min
.x
<= max
.x
&& min
.y
<= max
.y
&& min
.z
<= max
.z
);
3722 // return the volume of the AABB
3723 @property Float
volume () const {
3724 pragma(inline
, true);
3725 immutable diff
= max
-min
;
3726 static if (VT
.Dims
== 3) {
3727 return diff
.x
*diff
.y
*diff
.z
;
3729 return diff
.x
*diff
.y
;
3733 static auto mergeAABBs() (in auto ref Me aabb1
, in auto ref Me aabb2
) {
3735 res
.merge(aabb1
, aabb2
);
3739 void merge() (in auto ref Me aabb1
, in auto ref Me aabb2
) {
3740 pragma(inline
, true);
3741 min
.x
= nmin(aabb1
.min
.x
, aabb2
.min
.x
);
3742 min
.y
= nmin(aabb1
.min
.y
, aabb2
.min
.y
);
3743 max
.x
= nmax(aabb1
.max
.x
, aabb2
.max
.x
);
3744 max
.y
= nmax(aabb1
.max
.y
, aabb2
.max
.y
);
3745 static if (VT
.Dims
== 3) {
3746 min
.z
= nmin(aabb1
.min
.z
, aabb2
.min
.z
);
3747 max
.z
= nmax(aabb1
.max
.z
, aabb2
.max
.z
);
3751 void merge() (in auto ref Me aabb1
) {
3752 pragma(inline
, true);
3753 min
.x
= nmin(aabb1
.min
.x
, min
.x
);
3754 min
.y
= nmin(aabb1
.min
.y
, min
.y
);
3755 max
.x
= nmax(aabb1
.max
.x
, max
.x
);
3756 max
.y
= nmax(aabb1
.max
.y
, max
.y
);
3757 static if (VT
.Dims
== 3) {
3758 min
.z
= nmin(aabb1
.min
.z
, min
.z
);
3759 max
.z
= nmax(aabb1
.max
.z
, max
.z
);
3763 void addPoint() (in auto ref VT v
) {
3764 min
.x
= nmin(min
.x
, v
.x
);
3765 max
.x
= nmax(max
.x
, v
.x
);
3766 min
.y
= nmin(min
.y
, v
.y
);
3767 max
.y
= nmax(max
.y
, v
.y
);
3768 static if (VT
.Dims
== 3) {
3769 min
.z
= nmin(min
.z
, v
.z
);
3770 max
.z
= nmax(max
.z
, v
.z
);
3774 void opOpAssign(string op
:"~") (in auto ref VT v
) { pragma(inline
, true); addPoint(v
); }
3775 void opOpAssign(string op
:"~") (in auto ref Me aabb1
) { pragma(inline
, true); merge(aabb1
); }
3777 // return true if the current AABB contains the AABB given in parameter
3778 bool contains() (in auto ref Me aabb
) const {
3779 //pragma(inline, true);
3780 static if (VT
.Dims
== 3) {
3782 aabb
.min
.x
>= min
.x
&& aabb
.min
.y
>= min
.y
&& aabb
.min
.z
>= min
.z
&&
3783 aabb
.max
.x
<= max
.x
&& aabb
.max
.y
<= max
.y
&& aabb
.max
.z
<= max
.z
;
3786 aabb
.min
.x
>= min
.x
&& aabb
.min
.y
>= min
.y
&&
3787 aabb
.max
.x
<= max
.x
&& aabb
.max
.y
<= max
.y
;
3791 bool contains() (in auto ref VT p
) const {
3792 pragma(inline
, true);
3793 static if (VT
.Dims
== 2) {
3794 return (p
.x
>= min
.x
&& p
.y
>= min
.y
&& p
.x
<= max
.x
&& p
.y
<= max
.y
);
3796 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
);
3800 // extrude bbox a little, to compensate floating point inexactness
3802 void extrude (Float delta) pure {
3805 static if (VT.Dims == 3) min.z -= delta;
3808 static if (VT.Dims == 3) max.z += delta;
3812 // return true if the current AABB is overlapping with the AABB in parameter
3813 // two AABBs overlap if they overlap in the two(three) x, y (and z) axes at the same time
3814 bool overlaps() (in auto ref Me aabb
) const {
3815 //pragma(inline, true);
3816 // exit with no intersection if found separated along any axis
3817 if (max
.x
< aabb
.min
.x || min
.x
> aabb
.max
.x
) return false;
3818 if (max
.y
< aabb
.min
.y || min
.y
> aabb
.max
.y
) return false;
3819 static if (VT
.Dims
== 3) {
3820 if (max
.z
< aabb
.min
.z || min
.z
> aabb
.max
.z
) return false;
3825 // ////////////////////////////////////////////////////////////////////////// //
3826 // something to consider here is that 0 * inf =nan which occurs when the ray starts exactly on the edge of a box
3827 // rd: ray direction, normalized
3828 // https://tavianator.com/fast-branchless-raybounding-box-intersections-part-2-nans/
3829 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) {
3830 // ok with coplanars, but dmd sux at unrolled loops
3832 immutable Float dinvp0
= cast(Float
)1/ray
.dir
.x
; // 1/0 will produce inf
3833 immutable Float t1p0
= (bmin
.x
-ray
.orig
.x
)*dinvp0
;
3834 immutable Float t2p0
= (bmax
.x
-ray
.orig
.x
)*dinvp0
;
3835 Float tmin
= nmin(t1p0
, t2p0
);
3836 Float tmax
= nmax(t1p0
, t2p0
);
3839 immutable Float dinv
= cast(Float
)1/ray
.dir
.y
; // 1/0 will produce inf
3840 immutable Float t1
= (bmin
.y
-ray
.orig
.y
)*dinv
;
3841 immutable Float t2
= (bmax
.y
-ray
.orig
.y
)*dinv
;
3842 tmin
= nmax(tmin
, nmin(nmin(t1
, t2
), tmax
));
3843 tmax
= nmin(tmax
, nmax(nmax(t1
, t2
), tmin
));
3846 static if (VT
.Dims
== 3) {
3848 immutable Float dinv
= cast(Float
)1/ray
.dir
.z
; // 1/0 will produce inf
3849 immutable Float t1
= (bmin
.z
-ray
.orig
.z
)*dinv
;
3850 immutable Float t2
= (bmax
.z
-ray
.orig
.z
)*dinv
;
3851 tmin
= nmax(tmin
, nmin(nmin(t1
, t2
), tmax
));
3852 tmax
= nmin(tmax
, nmax(nmax(t1
, t2
), tmin
));
3855 if (tmax
> nmax(tmin
, cast(Float
)0)) {
3856 if (tmino
!is null) *tmino
= tmin
;
3857 if (tmaxo
!is null) *tmaxo
= tmax
;
3864 bool intersects() (in auto ref Ray
!VT ray
, Float
* tmino
=null, Float
* tmaxo
=null) const @trusted {
3865 // ok with coplanars, but dmd sux at unrolled loops
3867 immutable Float dinvp0
= cast(Float
)1/ray
.dir
.x
; // 1/0 will produce inf
3868 immutable Float t1p0
= (min
.x
-ray
.orig
.x
)*dinvp0
;
3869 immutable Float t2p0
= (max
.x
-ray
.orig
.x
)*dinvp0
;
3870 Float tmin
= nmin(t1p0
, t2p0
);
3871 Float tmax
= nmax(t1p0
, t2p0
);
3874 immutable Float dinv
= cast(Float
)1/ray
.dir
.y
; // 1/0 will produce inf
3875 immutable Float t1
= (min
.y
-ray
.orig
.y
)*dinv
;
3876 immutable Float t2
= (max
.y
-ray
.orig
.y
)*dinv
;
3877 tmin
= nmax(tmin
, nmin(nmin(t1
, t2
), tmax
));
3878 tmax
= nmin(tmax
, nmax(nmax(t1
, t2
), tmin
));
3881 static if (VT
.Dims
== 3) {
3883 immutable Float dinv
= cast(Float
)1/ray
.dir
.z
; // 1/0 will produce inf
3884 immutable Float t1
= (min
.z
-ray
.orig
.z
)*dinv
;
3885 immutable Float t2
= (max
.z
-ray
.orig
.z
)*dinv
;
3886 tmin
= nmax(tmin
, nmin(nmin(t1
, t2
), tmax
));
3887 tmax
= nmin(tmax
, nmax(nmax(t1
, t2
), tmin
));
3890 if (tmax
> nmax(tmin
, cast(Float
)0)) {
3891 if (tmino
!is null) *tmino
= tmin
;
3892 if (tmaxo
!is null) *tmaxo
= tmax
;
3899 Float
segIntersectMin() (in auto ref VT a
, in auto ref VT b
) const @trusted {
3901 if (!intersects(Ray
!VT
.fromPoints(a
, b
), &tmin
)) return -1;
3902 if (tmin
< 0) return 0; // inside
3903 if (tmin
*tmin
> (b
-a
).lengthSquared
) return -1;
3907 Float
segIntersectMax() (in auto ref VT a
, in auto ref VT b
) const @trusted {
3909 if (!intersects(Ray
!VT
.fromPoints(a
, b
), null, &tmax
)) return -1;
3910 if (tmax
*tmax
> (b
-a
).lengthSquared
) return -1;
3914 bool isIntersects() (in auto ref VT a
, in auto ref VT b
) const @trusted {
3915 // it may be faster to first check if start or end point is inside AABB (this is sometimes enough for dyntree)
3916 static if (VT
.Dims
== 2) {
3917 if (a
.x
>= min
.x
&& a
.y
>= min
.y
&& a
.x
<= max
.x
&& a
.y
<= max
.y
) return true; // a
3918 if (b
.x
>= min
.x
&& b
.y
>= min
.y
&& b
.x
<= max
.x
&& b
.y
<= max
.y
) return true; // b
3920 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
3921 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
3923 // nope, do it hard way
3925 if (!intersects(Ray
!VT
.fromPoints(a
, b
), &tmin
)) return false;
3926 if (tmin
< 0) return true; // inside, just in case
3927 return (tmin
*tmin
<= (b
-a
).lengthSquared
);
3930 ref inout(VT
) opIndex (usize idx
) inout {
3931 pragma(inline
, true);
3932 return (idx
== 0 ? min
: max
);
3935 /// sweep two AABB's to see if and when they are overlapping
3936 /// returns `true` if collision was detected (or boxes overlaps)
3937 /// u0 = normalized time of first collision (i.e. collision starts at myMove*u0)
3938 /// u1 = normalized time of second collision (i.e. collision stops after myMove*u1)
3939 /// hitnormal = normal that will move `this` apart of `b` edge it collided with
3940 /// no output values are valid if no collision was detected
3941 /// WARNING! hit normal calculation is not tested!
3942 bool sweep() (in auto ref VT myMove
, in auto ref Me b
, Float
* u0
, VT
* hitnormal
=null, Float
* u1
=null) const @trusted {
3943 // check if they are overlapping right now
3944 if (this.overlaps(b
)) {
3945 if (u0
!is null) *u0
= 0;
3946 if (u1
!is null) *u1
= 0;
3947 if (hitnormal
!is null) *hitnormal
= VT(); // oops
3951 immutable v
= -myMove
; // treat b as stationary, so invert v to get relative velocity
3953 // not moving, and not overlapping
3954 if (v
.isZero
) return false;
3958 Float
[VT
.Dims
] overlapTime
= 0;
3961 foreach (immutable aidx
; 0..VT
.Dims
) {
3963 immutable Float vv
= v
[aidx
];
3965 immutable Float invv
= cast(Float
)1/vv
;
3966 if (b
.max
[aidx
] < a
.min
[aidx
]) return false;
3967 if (b
.max
[aidx
] > a
.min
[aidx
]) outTime
= nmin((a
.min
[aidx
]-b
.max
[aidx
])*invv
, outTime
);
3968 if (a
.max
[aidx
] < b
.min
[aidx
]) hitTime
= nmax((overlapTime
.ptr
[aidx
] = (a
.max
[aidx
]-b
.min
[aidx
])*invv
), hitTime
);
3969 if (hitTime
> outTime
) return false;
3970 } else if (vv
> 0) {
3971 immutable Float invv
= cast(Float
)1/vv
;
3972 if (b
.min
[aidx
] > a
.max
[aidx
]) return false;
3973 if (a
.max
[aidx
] > b
.min
[aidx
]) outTime
= nmin((a
.max
[aidx
]-b
.min
[aidx
])*invv
, outTime
);
3974 if (b
.max
[aidx
] < a
.min
[aidx
]) hitTime
= nmax((overlapTime
.ptr
[aidx
] = (a
.min
[aidx
]-b
.max
[aidx
])*invv
), hitTime
);
3975 if (hitTime
> outTime
) return false;
3979 if (u0
!is null) *u0
= hitTime
;
3980 if (u1
!is null) *u1
= outTime
;
3982 // hit normal is along axis with the highest overlap time
3983 if (hitnormal
!is null) {
3984 static if (VT
.Dims
== 3) {
3986 if (overlapTime
.ptr
[1] > overlapTime
.ptr
[0]) aidx
= 1;
3987 if (overlapTime
.ptr
[2] > overlapTime
.ptr
[aidx
]) aidx
= 2;
3988 VT hn
; // zero vector
3989 hn
[aidx
] = (v
[aidx
] < 0 ?
-1 : v
[aidx
] > 0 ?
1 : 0);
3992 if (overlapTime
.ptr
[0] > overlapTime
.ptr
[1]) {
3993 *hitnormal
= VT((v
.x
< 0 ?
-1 : v
.x
> 0 ?
1 : 0), 0);
3995 *hitnormal
= VT(0, (v
.y
< 0 ?
-1 : v
.y
> 0 ?
1 : 0));
4004 auto u_0 = VT(0, 0, 0); // first times of overlap along each axis
4005 auto u_1 = VT(1, 1, 1); // last times of overlap along each axis
4006 bool wasHit = false;
4008 // find the possible first and last times of overlap along each axis
4009 foreach (immutable idx; 0..VT.Dims) {
4010 Float dinv = v[idx];
4012 dinv = cast(Float)1/dinv;
4013 if (this.max[idx] < b.min[idx] && dinv < 0) {
4014 u_0[idx] = (this.max[idx]-b.min[idx])*dinv;
4016 } else if (b.max[idx] < this.min[idx] && dinv > 0) {
4017 u_0[idx] = (this.min[idx]-b.max[idx])*dinv;
4020 if (b.max[idx] > this.min[idx] && dinv < 0) {
4021 u_1[idx] = (this.min[idx]-b.max[idx])*dinv;
4023 } else if (this.max[idx] > b.min[idx] && dinv > 0) {
4024 u_1[idx] = (this.max[idx]-b.min[idx])*dinv;
4032 if (u0 !is null) *u0 = 0;
4033 if (u1 !is null) *u1 = 0;
4037 static if (VT.Dims == 3) {
4038 immutable Float uu0 = nmax(u_0.x, nmax(u_0.y, u_0.z)); // possible first time of overlap
4039 immutable Float uu1 = nmin(u_1.x, nmin(u_1.y, u_1.z)); // possible last time of overlap
4041 immutable Float uu0 = nmax(u_0.x, u_0.y); // possible first time of overlap
4042 immutable Float uu1 = nmin(u_1.x, u_1.y); // possible last time of overlap
4045 if (u0 !is null) *u0 = uu0;
4046 if (u1 !is null) *u1 = uu1;
4048 // they could have only collided if the first time of overlap occurred before the last time of overlap
4049 return (uu0 <= uu1);
4054 /// check to see if the sphere overlaps the AABB
4055 bool overlaps(ST
) (in auto ref ST sphere
) if (IsSphere
!(ST
, VT
)) { pragma(inline
, true); return overlapsSphere(sphere
.orig
, sphere
.radius
); }
4057 /// check to see if the sphere overlaps the AABB
4058 bool overlapsSphere() (in auto ref VT center
, Float radius
) {
4060 // find the square of the distance from the sphere to the box
4061 foreach (immutable idx
; 0..VT
.Dims
) {
4062 if (center
[idx
] < min
[idx
]) {
4063 immutable Float s
= center
[idx
]-min
[idx
];
4065 } else if (center
[idx
] > max
[idx
]) {
4066 immutable Float s
= center
[idx
]-max
[idx
];
4070 return (d
<= radius
*radius
);
4075 // ////////////////////////////////////////////////////////////////////////// //
4076 /* Dynamic AABB tree (bounding volume hierarchy)
4077 * based on the code from ReactPhysics3D physics library, http://www.reactphysics3d.com
4078 * Copyright (c) 2010-2016 Daniel Chappuis
4080 * This software is provided 'as-is', without any express or implied warranty.
4081 * In no event will the authors be held liable for any damages arising from the
4082 * use of this software.
4084 * Permission is granted to anyone to use this software for any purpose,
4085 * including commercial applications, and to alter it and redistribute it
4086 * freely, subject to the following restrictions:
4088 * 1. The origin of this software must not be misrepresented; you must not claim
4089 * that you wrote the original software. If you use this software in a
4090 * product, an acknowledgment in the product documentation would be
4091 * appreciated but is not required.
4093 * 2. Altered source versions must be plainly marked as such, and must not be
4094 * misrepresented as being the original software.
4096 * 3. This notice may not be removed or altered from any source distribution.
4098 /* WARNING! BY DEFAULT TREE WILL NOT PROTECT OBJECTS FROM GC! */
4099 // ////////////////////////////////////////////////////////////////////////// //
4101 * This class implements a dynamic AABB tree that is used for broad-phase
4102 * collision detection. This data structure is inspired by Nathanael Presson's
4103 * dynamic tree implementation in BulletPhysics. The following implementation is
4104 * based on the one from Erin Catto in Box2D as described in the book
4105 * "Introduction to Game Physics with Box2D" by Ian Parberry.
4107 /// Dynamic AABB Tree: can be used to speed up broad phase in various engines
4108 /// GCAnchor==true: add nodes as GC roots; you won't need it if you're storing nodes in some other way
4109 public final class DynamicAABBTree(VT
, BodyBase
, bool GCAnchor
=false) if (IsVector
!VT
&& is(BodyBase
== class)) {
4111 static T
min(T
) (in T a
, in T b
) { pragma(inline
, true); return (a
< b ? a
: b
); }
4112 static T
max(T
) (in T a
, in T b
) { pragma(inline
, true); return (a
> b ? a
: b
); }
4116 alias Float
= VT
.Float
;
4117 alias Me
= typeof(this);
4118 alias AABB
= AABBImpl
!VT
;
4120 enum FloatNum(Float v
) = cast(Float
)v
;
4123 // in the broad-phase collision detection (dynamic AABB tree), the AABBs are
4124 // also inflated in direction of the linear motion of the body by mutliplying the
4125 // followin constant with the linear velocity and the elapsed time between two frames
4126 enum Float LinearMotionGapMultiplier
= FloatNum
!(1.7);
4129 // called when a overlapping node has been found during the call to forEachAABBOverlap()
4130 // return `true` to stop
4131 alias OverlapCallback
= void delegate (BodyBase abody
);
4132 alias SimpleQueryCallback
= bool delegate (BodyBase abody
);
4133 alias SimpleQueryCallbackNR
= void delegate (BodyBase abody
);
4134 alias SegQueryCallback
= Float
delegate (BodyBase abody
, in ref VT a
, in ref VT b
); // return dist from a to abody
4137 align(1) struct TreeNode
{
4139 enum NullTreeNode
= -1;
4140 enum { Left
= 0, Right
= 1 }
4141 // a node is either in the tree (has a parent) or in the free nodes list (has a next node)
4146 // a node is either a leaf (has data) or is an internal node (has children)
4148 int[2] children
; /// left and right child of the node (children[0] = left child)
4151 // height of the node in the tree (-1 for free nodes)
4153 // fat axis aligned bounding box (AABB) corresponding to the node
4155 // return true if the node is a leaf of the tree
4156 @property const pure nothrow @safe @nogc {
4157 bool leaf () { pragma(inline
, true); return (height
== 0); }
4158 bool free () { pragma(inline
, true); return (height
== -1); }
4163 TreeNode
* mNodes
; // pointer to the memory location of the nodes of the tree
4164 int mRootNodeId
; // id of the root node of the tree
4165 int mFreeNodeId
; // id of the first node of the list of free (allocated) nodes in the tree that we can use
4166 int mAllocCount
; // number of allocated nodes in the tree
4167 int mNodeCount
; // number of nodes in the tree
4169 // extra AABB Gap used to allow the collision shape to move a little bit
4170 // without triggering a large modification of the tree which can be costly
4174 // allocate and return a node to use in the tree
4175 int allocateNode () {
4176 // if there is no more allocated node to use
4177 if (mFreeNodeId
== TreeNode
.NullTreeNode
) {
4178 import core
.stdc
.stdlib
: realloc
;
4179 version(aabbtree_many_asserts
) assert(mNodeCount
== mAllocCount
);
4180 // allocate more nodes in the tree
4181 auto newsz
= (mAllocCount
< 8192 ? mAllocCount
*2 : mAllocCount
+8192);
4182 TreeNode
* nn
= cast(TreeNode
*)realloc(mNodes
, newsz
*TreeNode
.sizeof
);
4183 if (nn
is null) assert(0, "out of memory");
4184 //{ import core.stdc.stdio; printf("realloced: old=%u; new=%u\n", mAllocCount, newsz); }
4185 mAllocCount
= newsz
;
4187 // initialize the allocated nodes
4188 foreach (int i
; mNodeCount
..mAllocCount
-1) {
4189 mNodes
[i
].nextNodeId
= i
+1;
4190 mNodes
[i
].height
= -1;
4192 mNodes
[mAllocCount
-1].nextNodeId
= TreeNode
.NullTreeNode
;
4193 mNodes
[mAllocCount
-1].height
= -1;
4194 mFreeNodeId
= mNodeCount
;
4196 // get the next free node
4197 int freeNodeId
= mFreeNodeId
;
4198 version(aabbtree_many_asserts
) assert(freeNodeId
< mAllocCount
);
4199 mFreeNodeId
= mNodes
[freeNodeId
].nextNodeId
;
4200 mNodes
[freeNodeId
].parentId
= TreeNode
.NullTreeNode
;
4201 mNodes
[freeNodeId
].height
= 0;
4207 void releaseNode (int nodeId
) {
4208 version(aabbtree_many_asserts
) assert(mNodeCount
> 0);
4209 version(aabbtree_many_asserts
) assert(nodeId
>= 0 && nodeId
< mAllocCount
);
4210 version(aabbtree_many_asserts
) assert(mNodes
[nodeId
].height
>= 0);
4211 mNodes
[nodeId
].nextNodeId
= mFreeNodeId
;
4212 mNodes
[nodeId
].height
= -1;
4213 mFreeNodeId
= nodeId
;
4217 // insert a leaf node in the tree
4218 // 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
4219 void insertLeafNode (int nodeId
) {
4220 // if the tree is empty
4221 if (mRootNodeId
== TreeNode
.NullTreeNode
) {
4222 mRootNodeId
= nodeId
;
4223 mNodes
[mRootNodeId
].parentId
= TreeNode
.NullTreeNode
;
4227 version(aabbtree_many_asserts
) assert(mRootNodeId
!= TreeNode
.NullTreeNode
);
4229 // find the best sibling node for the new node
4230 AABB newNodeAABB
= mNodes
[nodeId
].aabb
;
4231 int currentNodeId
= mRootNodeId
;
4232 while (!mNodes
[currentNodeId
].leaf
) {
4233 int leftChild
= mNodes
[currentNodeId
].children
.ptr
[TreeNode
.Left
];
4234 int rightChild
= mNodes
[currentNodeId
].children
.ptr
[TreeNode
.Right
];
4236 // compute the merged AABB
4237 Float volumeAABB
= mNodes
[currentNodeId
].aabb
.volume
;
4238 AABB mergedAABBs
= AABB
.mergeAABBs(mNodes
[currentNodeId
].aabb
, newNodeAABB
);
4239 Float mergedVolume
= mergedAABBs
.volume
;
4241 // compute the cost of making the current node the sibling of the new node
4242 Float costS
= FloatNum
!2*mergedVolume
;
4244 // compute the minimum cost of pushing the new node further down the tree (inheritance cost)
4245 Float costI
= FloatNum
!2*(mergedVolume
-volumeAABB
);
4247 // compute the cost of descending into the left child
4248 AABB currentAndLeftAABB
= AABB
.mergeAABBs(newNodeAABB
, mNodes
[leftChild
].aabb
);
4249 Float costLeft
= currentAndLeftAABB
.volume
+costI
;
4250 if (!mNodes
[leftChild
].leaf
) costLeft
-= mNodes
[leftChild
].aabb
.volume
;
4252 // compute the cost of descending into the right child
4253 AABB currentAndRightAABB
= AABB
.mergeAABBs(newNodeAABB
, mNodes
[rightChild
].aabb
);
4254 Float costRight
= currentAndRightAABB
.volume
+costI
;
4255 if (!mNodes
[rightChild
].leaf
) costRight
-= mNodes
[rightChild
].aabb
.volume
;
4257 // 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
4258 if (costS
< costLeft
&& costS
< costRight
) break;
4260 // it is cheaper to go down into a child of the current node, choose the best child
4261 currentNodeId
= (costLeft
< costRight ? leftChild
: rightChild
);
4264 int siblingNode
= currentNodeId
;
4266 // create a new parent for the new node and the sibling node
4267 int oldParentNode
= mNodes
[siblingNode
].parentId
;
4268 int newParentNode
= allocateNode();
4269 mNodes
[newParentNode
].parentId
= oldParentNode
;
4270 mNodes
[newParentNode
].aabb
.merge(mNodes
[siblingNode
].aabb
, newNodeAABB
);
4271 mNodes
[newParentNode
].height
= cast(short)(mNodes
[siblingNode
].height
+1);
4272 version(aabbtree_many_asserts
) assert(mNodes
[newParentNode
].height
> 0);
4274 // if the sibling node was not the root node
4275 if (oldParentNode
!= TreeNode
.NullTreeNode
) {
4276 version(aabbtree_many_asserts
) assert(!mNodes
[oldParentNode
].leaf
);
4277 if (mNodes
[oldParentNode
].children
.ptr
[TreeNode
.Left
] == siblingNode
) {
4278 mNodes
[oldParentNode
].children
.ptr
[TreeNode
.Left
] = newParentNode
;
4280 mNodes
[oldParentNode
].children
.ptr
[TreeNode
.Right
] = newParentNode
;
4282 mNodes
[newParentNode
].children
.ptr
[TreeNode
.Left
] = siblingNode
;
4283 mNodes
[newParentNode
].children
.ptr
[TreeNode
.Right
] = nodeId
;
4284 mNodes
[siblingNode
].parentId
= newParentNode
;
4285 mNodes
[nodeId
].parentId
= newParentNode
;
4287 // if the sibling node was the root node
4288 mNodes
[newParentNode
].children
.ptr
[TreeNode
.Left
] = siblingNode
;
4289 mNodes
[newParentNode
].children
.ptr
[TreeNode
.Right
] = nodeId
;
4290 mNodes
[siblingNode
].parentId
= newParentNode
;
4291 mNodes
[nodeId
].parentId
= newParentNode
;
4292 mRootNodeId
= newParentNode
;
4295 // move up in the tree to change the AABBs that have changed
4296 currentNodeId
= mNodes
[nodeId
].parentId
;
4297 version(aabbtree_many_asserts
) assert(!mNodes
[currentNodeId
].leaf
);
4298 while (currentNodeId
!= TreeNode
.NullTreeNode
) {
4299 // balance the sub-tree of the current node if it is not balanced
4300 currentNodeId
= balanceSubTreeAtNode(currentNodeId
);
4301 version(aabbtree_many_asserts
) assert(mNodes
[nodeId
].leaf
);
4303 version(aabbtree_many_asserts
) assert(!mNodes
[currentNodeId
].leaf
);
4304 int leftChild
= mNodes
[currentNodeId
].children
.ptr
[TreeNode
.Left
];
4305 int rightChild
= mNodes
[currentNodeId
].children
.ptr
[TreeNode
.Right
];
4306 version(aabbtree_many_asserts
) assert(leftChild
!= TreeNode
.NullTreeNode
);
4307 version(aabbtree_many_asserts
) assert(rightChild
!= TreeNode
.NullTreeNode
);
4309 // recompute the height of the node in the tree
4310 mNodes
[currentNodeId
].height
= cast(short)(max(mNodes
[leftChild
].height
, mNodes
[rightChild
].height
)+1);
4311 version(aabbtree_many_asserts
) assert(mNodes
[currentNodeId
].height
> 0);
4313 // recompute the AABB of the node
4314 mNodes
[currentNodeId
].aabb
.merge(mNodes
[leftChild
].aabb
, mNodes
[rightChild
].aabb
);
4316 currentNodeId
= mNodes
[currentNodeId
].parentId
;
4319 version(aabbtree_many_asserts
) assert(mNodes
[nodeId
].leaf
);
4322 // remove a leaf node from the tree
4323 void removeLeafNode (int nodeId
) {
4324 version(aabbtree_many_asserts
) assert(nodeId
>= 0 && nodeId
< mAllocCount
);
4325 version(aabbtree_many_asserts
) assert(mNodes
[nodeId
].leaf
);
4327 // if we are removing the root node (root node is a leaf in this case)
4328 if (mRootNodeId
== nodeId
) { mRootNodeId
= TreeNode
.NullTreeNode
; return; }
4330 int parentNodeId
= mNodes
[nodeId
].parentId
;
4331 int grandParentNodeId
= mNodes
[parentNodeId
].parentId
;
4334 if (mNodes
[parentNodeId
].children
.ptr
[TreeNode
.Left
] == nodeId
) {
4335 siblingNodeId
= mNodes
[parentNodeId
].children
.ptr
[TreeNode
.Right
];
4337 siblingNodeId
= mNodes
[parentNodeId
].children
.ptr
[TreeNode
.Left
];
4340 // if the parent of the node to remove is not the root node
4341 if (grandParentNodeId
!= TreeNode
.NullTreeNode
) {
4342 // destroy the parent node
4343 if (mNodes
[grandParentNodeId
].children
.ptr
[TreeNode
.Left
] == parentNodeId
) {
4344 mNodes
[grandParentNodeId
].children
.ptr
[TreeNode
.Left
] = siblingNodeId
;
4346 version(aabbtree_many_asserts
) assert(mNodes
[grandParentNodeId
].children
.ptr
[TreeNode
.Right
] == parentNodeId
);
4347 mNodes
[grandParentNodeId
].children
.ptr
[TreeNode
.Right
] = siblingNodeId
;
4349 mNodes
[siblingNodeId
].parentId
= grandParentNodeId
;
4350 releaseNode(parentNodeId
);
4352 // 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
4353 int currentNodeId
= grandParentNodeId
;
4354 while (currentNodeId
!= TreeNode
.NullTreeNode
) {
4355 // balance the current sub-tree if necessary
4356 currentNodeId
= balanceSubTreeAtNode(currentNodeId
);
4358 version(aabbtree_many_asserts
) assert(!mNodes
[currentNodeId
].leaf
);
4360 // get the two children.ptr of the current node
4361 int leftChildId
= mNodes
[currentNodeId
].children
.ptr
[TreeNode
.Left
];
4362 int rightChildId
= mNodes
[currentNodeId
].children
.ptr
[TreeNode
.Right
];
4364 // recompute the AABB and the height of the current node
4365 mNodes
[currentNodeId
].aabb
.merge(mNodes
[leftChildId
].aabb
, mNodes
[rightChildId
].aabb
);
4366 mNodes
[currentNodeId
].height
= cast(short)(max(mNodes
[leftChildId
].height
, mNodes
[rightChildId
].height
)+1);
4367 version(aabbtree_many_asserts
) assert(mNodes
[currentNodeId
].height
> 0);
4369 currentNodeId
= mNodes
[currentNodeId
].parentId
;
4372 // if the parent of the node to remove is the root node, the sibling node becomes the new root node
4373 mRootNodeId
= siblingNodeId
;
4374 mNodes
[siblingNodeId
].parentId
= TreeNode
.NullTreeNode
;
4375 releaseNode(parentNodeId
);
4379 // balance the sub-tree of a given node using left or right rotations
4380 // the rotation schemes are described in the book "Introduction to Game Physics with Box2D" by Ian Parberry
4381 // this method returns the new root node id
4382 int balanceSubTreeAtNode (int nodeId
) {
4383 version(aabbtree_many_asserts
) assert(nodeId
!= TreeNode
.NullTreeNode
);
4385 TreeNode
* nodeA
= mNodes
+nodeId
;
4387 // if the node is a leaf or the height of A's sub-tree is less than 2
4388 if (nodeA
.leaf || nodeA
.height
< 2) return nodeId
; // do not perform any rotation
4390 // get the two children nodes
4391 int nodeBId
= nodeA
.children
.ptr
[TreeNode
.Left
];
4392 int nodeCId
= nodeA
.children
.ptr
[TreeNode
.Right
];
4393 version(aabbtree_many_asserts
) assert(nodeBId
>= 0 && nodeBId
< mAllocCount
);
4394 version(aabbtree_many_asserts
) assert(nodeCId
>= 0 && nodeCId
< mAllocCount
);
4395 TreeNode
* nodeB
= mNodes
+nodeBId
;
4396 TreeNode
* nodeC
= mNodes
+nodeCId
;
4398 // compute the factor of the left and right sub-trees
4399 int balanceFactor
= nodeC
.height
-nodeB
.height
;
4401 // if the right node C is 2 higher than left node B
4402 if (balanceFactor
> 1) {
4403 version(aabbtree_many_asserts
) assert(!nodeC
.leaf
);
4405 int nodeFId
= nodeC
.children
.ptr
[TreeNode
.Left
];
4406 int nodeGId
= nodeC
.children
.ptr
[TreeNode
.Right
];
4407 version(aabbtree_many_asserts
) assert(nodeFId
>= 0 && nodeFId
< mAllocCount
);
4408 version(aabbtree_many_asserts
) assert(nodeGId
>= 0 && nodeGId
< mAllocCount
);
4409 TreeNode
* nodeF
= mNodes
+nodeFId
;
4410 TreeNode
* nodeG
= mNodes
+nodeGId
;
4412 nodeC
.children
.ptr
[TreeNode
.Left
] = nodeId
;
4413 nodeC
.parentId
= nodeA
.parentId
;
4414 nodeA
.parentId
= nodeCId
;
4416 if (nodeC
.parentId
!= TreeNode
.NullTreeNode
) {
4417 if (mNodes
[nodeC
.parentId
].children
.ptr
[TreeNode
.Left
] == nodeId
) {
4418 mNodes
[nodeC
.parentId
].children
.ptr
[TreeNode
.Left
] = nodeCId
;
4420 version(aabbtree_many_asserts
) assert(mNodes
[nodeC
.parentId
].children
.ptr
[TreeNode
.Right
] == nodeId
);
4421 mNodes
[nodeC
.parentId
].children
.ptr
[TreeNode
.Right
] = nodeCId
;
4424 mRootNodeId
= nodeCId
;
4427 version(aabbtree_many_asserts
) assert(!nodeC
.leaf
);
4428 version(aabbtree_many_asserts
) assert(!nodeA
.leaf
);
4430 // if the right node C was higher than left node B because of the F node
4431 if (nodeF
.height
> nodeG
.height
) {
4432 nodeC
.children
.ptr
[TreeNode
.Right
] = nodeFId
;
4433 nodeA
.children
.ptr
[TreeNode
.Right
] = nodeGId
;
4434 nodeG
.parentId
= nodeId
;
4436 // recompute the AABB of node A and C
4437 nodeA
.aabb
.merge(nodeB
.aabb
, nodeG
.aabb
);
4438 nodeC
.aabb
.merge(nodeA
.aabb
, nodeF
.aabb
);
4440 // recompute the height of node A and C
4441 nodeA
.height
= cast(short)(max(nodeB
.height
, nodeG
.height
)+1);
4442 nodeC
.height
= cast(short)(max(nodeA
.height
, nodeF
.height
)+1);
4443 version(aabbtree_many_asserts
) assert(nodeA
.height
> 0);
4444 version(aabbtree_many_asserts
) assert(nodeC
.height
> 0);
4446 // if the right node C was higher than left node B because of node G
4447 nodeC
.children
.ptr
[TreeNode
.Right
] = nodeGId
;
4448 nodeA
.children
.ptr
[TreeNode
.Right
] = nodeFId
;
4449 nodeF
.parentId
= nodeId
;
4451 // recompute the AABB of node A and C
4452 nodeA
.aabb
.merge(nodeB
.aabb
, nodeF
.aabb
);
4453 nodeC
.aabb
.merge(nodeA
.aabb
, nodeG
.aabb
);
4455 // recompute the height of node A and C
4456 nodeA
.height
= cast(short)(max(nodeB
.height
, nodeF
.height
)+1);
4457 nodeC
.height
= cast(short)(max(nodeA
.height
, nodeG
.height
)+1);
4458 version(aabbtree_many_asserts
) assert(nodeA
.height
> 0);
4459 version(aabbtree_many_asserts
) assert(nodeC
.height
> 0);
4462 // return the new root of the sub-tree
4466 // if the left node B is 2 higher than right node C
4467 if (balanceFactor
< -1) {
4468 version(aabbtree_many_asserts
) assert(!nodeB
.leaf
);
4470 int nodeFId
= nodeB
.children
.ptr
[TreeNode
.Left
];
4471 int nodeGId
= nodeB
.children
.ptr
[TreeNode
.Right
];
4472 version(aabbtree_many_asserts
) assert(nodeFId
>= 0 && nodeFId
< mAllocCount
);
4473 version(aabbtree_many_asserts
) assert(nodeGId
>= 0 && nodeGId
< mAllocCount
);
4474 TreeNode
* nodeF
= mNodes
+nodeFId
;
4475 TreeNode
* nodeG
= mNodes
+nodeGId
;
4477 nodeB
.children
.ptr
[TreeNode
.Left
] = nodeId
;
4478 nodeB
.parentId
= nodeA
.parentId
;
4479 nodeA
.parentId
= nodeBId
;
4481 if (nodeB
.parentId
!= TreeNode
.NullTreeNode
) {
4482 if (mNodes
[nodeB
.parentId
].children
.ptr
[TreeNode
.Left
] == nodeId
) {
4483 mNodes
[nodeB
.parentId
].children
.ptr
[TreeNode
.Left
] = nodeBId
;
4485 version(aabbtree_many_asserts
) assert(mNodes
[nodeB
.parentId
].children
.ptr
[TreeNode
.Right
] == nodeId
);
4486 mNodes
[nodeB
.parentId
].children
.ptr
[TreeNode
.Right
] = nodeBId
;
4489 mRootNodeId
= nodeBId
;
4492 version(aabbtree_many_asserts
) assert(!nodeB
.leaf
);
4493 version(aabbtree_many_asserts
) assert(!nodeA
.leaf
);
4495 // if the left node B was higher than right node C because of the F node
4496 if (nodeF
.height
> nodeG
.height
) {
4497 nodeB
.children
.ptr
[TreeNode
.Right
] = nodeFId
;
4498 nodeA
.children
.ptr
[TreeNode
.Left
] = nodeGId
;
4499 nodeG
.parentId
= nodeId
;
4501 // recompute the AABB of node A and B
4502 nodeA
.aabb
.merge(nodeC
.aabb
, nodeG
.aabb
);
4503 nodeB
.aabb
.merge(nodeA
.aabb
, nodeF
.aabb
);
4505 // recompute the height of node A and B
4506 nodeA
.height
= cast(short)(max(nodeC
.height
, nodeG
.height
)+1);
4507 nodeB
.height
= cast(short)(max(nodeA
.height
, nodeF
.height
)+1);
4508 version(aabbtree_many_asserts
) assert(nodeA
.height
> 0);
4509 version(aabbtree_many_asserts
) assert(nodeB
.height
> 0);
4511 // if the left node B was higher than right node C because of node G
4512 nodeB
.children
.ptr
[TreeNode
.Right
] = nodeGId
;
4513 nodeA
.children
.ptr
[TreeNode
.Left
] = nodeFId
;
4514 nodeF
.parentId
= nodeId
;
4516 // recompute the AABB of node A and B
4517 nodeA
.aabb
.merge(nodeC
.aabb
, nodeF
.aabb
);
4518 nodeB
.aabb
.merge(nodeA
.aabb
, nodeG
.aabb
);
4520 // recompute the height of node A and B
4521 nodeA
.height
= cast(short)(max(nodeC
.height
, nodeF
.height
)+1);
4522 nodeB
.height
= cast(short)(max(nodeA
.height
, nodeG
.height
)+1);
4523 version(aabbtree_many_asserts
) assert(nodeA
.height
> 0);
4524 version(aabbtree_many_asserts
) assert(nodeB
.height
> 0);
4527 // return the new root of the sub-tree
4531 // if the sub-tree is balanced, return the current root node
4535 // compute the height of a given node in the tree
4536 int computeHeight (int nodeId
) {
4537 version(aabbtree_many_asserts
) assert(nodeId
>= 0 && nodeId
< mAllocCount
);
4538 TreeNode
* node
= mNodes
+nodeId
;
4540 // if the node is a leaf, its height is zero
4541 if (node
.leaf
) return 0;
4543 // compute the height of the left and right sub-tree
4544 int leftHeight
= computeHeight(node
.children
.ptr
[TreeNode
.Left
]);
4545 int rightHeight
= computeHeight(node
.children
.ptr
[TreeNode
.Right
]);
4547 // return the height of the node
4548 return 1+max(leftHeight
, rightHeight
);
4551 // internally add an object into the tree
4552 int insertObjectInternal (in ref AABB aabb
, bool staticObject
) {
4553 // get the next available node (or allocate new ones if necessary)
4554 int nodeId
= allocateNode();
4556 // create the fat aabb to use in the tree
4557 mNodes
[nodeId
].aabb
= aabb
;
4558 if (!staticObject
) {
4559 static if (VT
.Dims
== 2) {
4560 immutable gap
= VT(mExtraGap
, mExtraGap
);
4562 immutable gap
= VT(mExtraGap
, mExtraGap
, mExtraGap
);
4564 mNodes
[nodeId
].aabb
.min
-= gap
;
4565 mNodes
[nodeId
].aabb
.max
+= gap
;
4568 // set the height of the node in the tree
4569 mNodes
[nodeId
].height
= 0;
4571 // insert the new leaf node in the tree
4572 insertLeafNode(nodeId
);
4573 version(aabbtree_many_asserts
) assert(mNodes
[nodeId
].leaf
);
4575 version(aabbtree_many_asserts
) assert(nodeId
>= 0);
4577 // return the id of the node
4581 // initialize the tree
4583 import core
.stdc
.stdlib
: malloc
;
4584 import core
.stdc
.string
: memset
;
4586 mRootNodeId
= TreeNode
.NullTreeNode
;
4590 mNodes
= cast(TreeNode
*)malloc(mAllocCount
*TreeNode
.sizeof
);
4591 if (mNodes
is null) assert(0, "out of memory");
4592 memset(mNodes
, 0, mAllocCount
*TreeNode
.sizeof
);
4594 // initialize the allocated nodes
4595 foreach (int i
; 0..mAllocCount
-1) {
4596 mNodes
[i
].nextNodeId
= i
+1;
4597 mNodes
[i
].height
= -1;
4599 mNodes
[mAllocCount
-1].nextNodeId
= TreeNode
.NullTreeNode
;
4600 mNodes
[mAllocCount
-1].height
= -1;
4604 // also, checks if the tree structure is valid (for debugging purpose)
4605 public void forEachLeaf (scope void delegate (BodyBase abody
, in ref AABB aabb
) dg
) {
4606 void forEachNode (int nodeId
) {
4607 if (nodeId
== TreeNode
.NullTreeNode
) return;
4608 // if it is the root
4609 if (nodeId
== mRootNodeId
) {
4610 assert(mNodes
[nodeId
].parentId
== TreeNode
.NullTreeNode
);
4612 // get the children nodes
4613 TreeNode
* pNode
= mNodes
+nodeId
;
4614 assert(pNode
.height
>= 0);
4615 assert(pNode
.aabb
.volume
> 0);
4616 // if the current node is a leaf
4618 assert(pNode
.height
== 0);
4619 if (dg
!is null) dg(pNode
.flesh
, pNode
.aabb
);
4621 int leftChild
= pNode
.children
.ptr
[TreeNode
.Left
];
4622 int rightChild
= pNode
.children
.ptr
[TreeNode
.Right
];
4623 // check that the children node Ids are valid
4624 assert(0 <= leftChild
&& leftChild
< mAllocCount
);
4625 assert(0 <= rightChild
&& rightChild
< mAllocCount
);
4626 // check that the children nodes have the correct parent node
4627 assert(mNodes
[leftChild
].parentId
== nodeId
);
4628 assert(mNodes
[rightChild
].parentId
== nodeId
);
4629 // check the height of node
4630 int height
= 1+max(mNodes
[leftChild
].height
, mNodes
[rightChild
].height
);
4631 assert(mNodes
[nodeId
].height
== height
);
4632 // check the AABB of the node
4633 AABB aabb
= AABB
.mergeAABBs(mNodes
[leftChild
].aabb
, mNodes
[rightChild
].aabb
);
4634 assert(aabb
.min
== mNodes
[nodeId
].aabb
.min
);
4635 assert(aabb
.max
== mNodes
[nodeId
].aabb
.max
);
4636 // recursively check the children nodes
4637 forEachNode(leftChild
);
4638 forEachNode(rightChild
);
4641 // recursively check each node
4642 forEachNode(mRootNodeId
);
4645 static if (GCAnchor
) void gcRelease () {
4646 import core
.memory
: GC
;
4647 foreach (ref TreeNode n
; mNodes
[0..mNodeCount
]) {
4649 auto flesh
= n
.flesh
;
4650 GC
.clrAttr(*cast(void**)&flesh
, GC
.BlkAttr
.NO_MOVE
);
4651 GC
.removeRoot(*cast(void**)&flesh
);
4656 version(aabbtree_query_count
) public int nodesVisited
, nodesDeepVisited
;
4658 // return `true` from visitor to stop immediately
4659 // checker should check if this node should be considered to further checking
4660 // returns tree node if visitor says stop or -1
4661 private int visit (scope bool delegate (TreeNode
* node
) checker
, scope bool delegate (BodyBase abody
) visitor
) {
4662 int[1024] stack
= void; // stack with the nodes to visit
4664 int[] bigstack
= null;
4665 scope(exit
) if (bigstack
.ptr
!is null) delete bigstack
;
4667 void spush (int id
) {
4668 if (sp
< stack
.length
) {
4669 // use "small stack"
4670 stack
.ptr
[sp
++] = id
;
4672 if (sp
>= int.max
/2) assert(0, "huge tree!");
4674 immutable int xsp
= sp
-cast(int)stack
.length
;
4675 if (xsp
< bigstack
.length
) {
4677 bigstack
.ptr
[xsp
] = id
;
4680 auto optr
= bigstack
.ptr
;
4682 if (bigstack
.ptr
!is optr
) {
4683 import core
.memory
: GC
;
4684 optr
= bigstack
.ptr
;
4685 if (optr
is GC
.addrOf(optr
)) GC
.setAttr(optr
, GC
.BlkAttr
.NO_INTERIOR
);
4693 pragma(inline
, true); // why not?
4694 if (sp
== 0) assert(0, "stack underflow");
4695 if (sp
<= stack
.length
) {
4696 // use "small stack"
4697 return stack
.ptr
[--sp
];
4701 return bigstack
.ptr
[sp
-cast(int)stack
.length
];
4705 version(aabbtree_query_count
) nodesVisited
= nodesDeepVisited
= 0;
4707 // start from root node
4710 // while there are still nodes to visit
4712 // get the next node id to visit
4713 int nodeId
= spop();
4714 // skip it if it is a null node
4715 if (nodeId
== TreeNode
.NullTreeNode
) continue;
4716 version(aabbtree_query_count
) ++nodesVisited
;
4717 // get the corresponding node
4718 TreeNode
* node
= mNodes
+nodeId
;
4719 // should we investigate this node?
4720 if (checker(node
)) {
4721 // if the node is a leaf
4723 // call visitor on it
4724 version(aabbtree_query_count
) ++nodesDeepVisited
;
4725 if (visitor(node
.flesh
)) return nodeId
;
4727 // if the node is not a leaf, we need to visit its children
4728 spush(node
.children
.ptr
[TreeNode
.Left
]);
4729 spush(node
.children
.ptr
[TreeNode
.Right
]);
4738 /// add `extraAABBGap` to bounding boxes so slight object movement won't cause tree rebuilds
4739 /// 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
4740 this (Float extraAABBGap
=FloatNum
!0) nothrow {
4741 mExtraGap
= extraAABBGap
;
4746 import core
.stdc
.stdlib
: free
;
4747 static if (GCAnchor
) gcRelease();
4751 /// return the root AABB of the tree
4752 AABB
getRootAABB () nothrow @trusted @nogc {
4753 pragma(inline
, true);
4754 version(aabbtree_many_asserts
) assert(mRootNodeId
>= 0 && mRootNodeId
< mAllocCount
);
4755 return mNodes
[mRootNodeId
].aabb
;
4758 /// does the given id represents a valid object?
4759 /// WARNING: ids of removed objects can be reused on later insertions!
4760 bool isValidId (int id
) const nothrow @trusted @nogc { pragma(inline
, true); return (id
>= 0 && id
< mAllocCount
&& mNodes
[id
].leaf
); }
4762 /// get current extra AABB gap
4763 @property Float
extraGap () const pure nothrow @trusted @nogc { pragma(inline
, true); return mExtraGap
; }
4765 /// set current extra AABB gap
4766 @property void extraGap (Float aExtraGap
) pure nothrow @trusted @nogc { pragma(inline
, true); mExtraGap
= aExtraGap
; }
4768 /// get object by id; can return null for invalid ids
4769 BodyBase
getObject (int id
) nothrow @trusted @nogc { pragma(inline
, true); return (id
>= 0 && id
< mAllocCount
&& mNodes
[id
].leaf ? mNodes
[id
].flesh
: null); }
4771 /// get fat object AABB by id; returns random shit for invalid ids
4772 AABB
getObjectFatAABB (int id
) nothrow @trusted @nogc { pragma(inline
, true); return (id
>= 0 && id
< mAllocCount
&& !mNodes
[id
].free ? mNodes
[id
].aabb
: AABB()); }
4774 /// insert an object into the tree
4775 /// this method creates a new leaf node in the tree and returns the id of the corresponding node
4776 /// AABB for static object will not be "fat" (simple optimization)
4777 /// WARNING! inserting the same object several times *WILL* break everything!
4778 int insertObject (BodyBase flesh
, bool staticObject
=false) {
4779 auto aabb
= flesh
.getAABB(); // can be passed as argument
4780 int nodeId
= insertObjectInternal(aabb
, staticObject
);
4781 version(aabbtree_many_asserts
) assert(mNodes
[nodeId
].leaf
);
4782 mNodes
[nodeId
].flesh
= flesh
;
4783 static if (GCAnchor
) {
4784 import core
.memory
: GC
;
4785 GC
.addRoot(*cast(void**)&flesh
);
4786 GC
.setAttr(*cast(void**)&flesh
, GC
.BlkAttr
.NO_MOVE
);
4791 /// remove an object from the tree
4792 /// WARNING: ids of removed objects can be reused on later insertions!
4793 void removeObject (int nodeId
) {
4794 if (nodeId
< 0 || nodeId
>= mAllocCount ||
!mNodes
[nodeId
].leaf
) assert(0, "invalid node id");
4795 static if (GCAnchor
) {
4796 import core
.memory
: GC
;
4797 auto flesh
= mNodes
[nodeId
].flesh
;
4798 GC
.clrAttr(*cast(void**)&flesh
, GC
.BlkAttr
.NO_MOVE
);
4799 GC
.removeRoot(*cast(void**)&flesh
);
4801 // remove the node from the tree
4802 removeLeafNode(nodeId
);
4803 releaseNode(nodeId
);
4806 /** update the dynamic tree after an object has moved.
4808 * if the new AABB of the object that has moved is still inside its fat AABB, then nothing is done.
4809 * otherwise, the corresponding node is removed and reinserted into the tree.
4810 * the method returns true if the object has been reinserted into the tree.
4811 * the "displacement" parameter is the linear velocity of the AABB multiplied by the elapsed time between two frames.
4812 * if the "forceReinsert" parameter is true, we force a removal and reinsertion of the node
4813 * (this can be useful if the shape AABB has become much smaller than the previous one for instance).
4815 * note that you should call this method if body's AABB was modified, even if the body wasn't moved.
4817 * if `forceReinsert` == `true` and `displacement` is zero, convert object to "static" (don't extrude AABB).
4819 * return `true` if the tree was modified.
4821 bool updateObject() (int nodeId
, in auto ref AABB
.VType displacement
, bool forceReinsert
=false) {
4822 if (nodeId
< 0 || nodeId
>= mAllocCount ||
!mNodes
[nodeId
].leaf
) assert(0, "invalid node id");
4824 auto newAABB
= mNodes
[nodeId
].flesh
.getAABB(); // can be passed as argument
4826 // if the new AABB is still inside the fat AABB of the node
4827 if (!forceReinsert
&& mNodes
[nodeId
].aabb
.contains(newAABB
)) return false;
4829 // if the new AABB is outside the fat AABB, we remove the corresponding node
4830 removeLeafNode(nodeId
);
4832 // compute the fat AABB by inflating the AABB with a constant gap
4833 mNodes
[nodeId
].aabb
= newAABB
;
4834 if (!(forceReinsert
&& displacement
.isZero
)) {
4835 static if (VT
.Dims
== 2) {
4836 immutable gap
= VT(mExtraGap
, mExtraGap
);
4838 immutable gap
= VT(mExtraGap
, mExtraGap
, mExtraGap
);
4840 mNodes
[nodeId
].aabb
.mMin
-= gap
;
4841 mNodes
[nodeId
].aabb
.mMax
+= gap
;
4844 // inflate the fat AABB in direction of the linear motion of the AABB
4845 if (displacement
.x
< FloatNum
!0) {
4846 mNodes
[nodeId
].aabb
.mMin
.x
+= LinearMotionGapMultiplier
*displacement
.x
;
4848 mNodes
[nodeId
].aabb
.mMax
.x
+= LinearMotionGapMultiplier
*displacement
.x
;
4850 if (displacement
.y
< FloatNum
!0) {
4851 mNodes
[nodeId
].aabb
.mMin
.y
+= LinearMotionGapMultiplier
*displacement
.y
;
4853 mNodes
[nodeId
].aabb
.mMax
.y
+= LinearMotionGapMultiplier
*displacement
.y
;
4855 static if (AABB
.VType
.Dims
== 3) {
4856 if (displacement
.z
< FloatNum
!0) {
4857 mNodes
[nodeId
].aabb
.mMin
.z
+= LinearMotionGapMultiplier
*displacement
.z
;
4859 mNodes
[nodeId
].aabb
.mMax
.z
+= LinearMotionGapMultiplier
*displacement
.z
;
4863 version(aabbtree_many_asserts
) assert(mNodes
[nodeId
].aabb
.contains(newAABB
));
4865 // reinsert the node into the tree
4866 insertLeafNode(nodeId
);
4871 /// report all shapes overlapping with the AABB given in parameter
4872 void forEachAABBOverlap() (in auto ref AABB aabb
, scope OverlapCallback cb
) {
4875 (node
) => aabb
.overlaps(node
.aabb
),
4877 (flesh
) { cb(flesh
); return false; }
4881 /// report body that contains the given point
4882 BodyBase
pointQuery() (in auto ref VT point
, scope SimpleQueryCallback cb
) {
4885 (node
) => node
.aabb
.contains(point
),
4887 (flesh
) => cb(flesh
),
4889 version(aabbtree_many_asserts
) assert(nid
< 0 ||
(nid
>= 0 && nid
< mAllocCount
&& mNodes
[nid
].leaf
));
4890 return (nid
>= 0 ? mNodes
[nid
].flesh
: null);
4893 /// report all bodies containing the given point
4894 void pointQuery() (in auto ref VT point
, scope SimpleQueryCallbackNR cb
) {
4897 (node
) => node
.aabb
.contains(point
),
4899 (flesh
) { cb(flesh
); return false; },
4904 static struct SegmentQueryResult
{
4905 Float dist
= -1; /// <0: nothing was hit
4908 @property bool valid () const nothrow @safe @nogc { pragma(inline
, true); return (dist
>= 0 && flesh
!is null); } ///
4911 /// segment querying method
4912 SegmentQueryResult
segmentQuery() (in auto ref VT a
, in auto ref VT b
, scope SegQueryCallback cb
) {
4913 SegmentQueryResult res
;
4914 Float maxFraction
= Float
.infinity
;
4916 immutable VT cura
= a
;
4918 immutable VT dir
= (curb
-cura
).normalized
;
4922 (node
) => node
.aabb
.isIntersects(cura
, curb
),
4925 Float hitFraction
= cb(flesh
, cura
, curb
);
4926 // if the user returned a hitFraction of zero, it means that the raycasting should stop here
4927 if (hitFraction
== FloatNum
!0) {
4932 // if the user returned a positive fraction
4933 if (hitFraction
> FloatNum
!0) {
4934 // we update the maxFraction value and the ray AABB using the new maximum fraction
4935 if (hitFraction
< maxFraction
) {
4936 maxFraction
= hitFraction
;
4937 res
.dist
= hitFraction
;
4940 curb
= cura
+dir
*hitFraction
;
4943 return false; // continue
4950 /// compute the height of the tree
4951 int computeHeight () { pragma(inline
, true); return computeHeight(mRootNodeId
); }
4953 @property int nodeCount () const pure nothrow @safe @nogc { pragma(inline
, true); return mNodeCount
; }
4954 @property int nodeAlloced () const pure nothrow @safe @nogc { pragma(inline
, true); return mAllocCount
; }
4956 /// clear all the nodes and reset the tree
4958 import core
.stdc
.stdlib
: free
;
4959 static if (GCAnchor
) gcRelease();
4967 // ////////////////////////////////////////////////////////////////////////// //
4971 this (vec2 amin, vec2 amax) { aabb.min = amin; aabb.max = amax; }
4973 AABB getAABB () { return aabb; }
4977 // ////////////////////////////////////////////////////////////////////////// //
4981 auto tree = new DynamicAABBTree!Body(0.2);
4983 vec2 bmin = vec2(10, 15);
4984 vec2 bmax = vec2(42, 54);
4986 auto flesh = new Body(bmin, bmax);
4988 tree.insertObject(flesh);
4990 vec2 ro = vec2(5, 18);
4991 vec2 rd = vec2(1, 0.2).normalized;
4994 writeln(flesh.aabb.segIntersectMin(ro, re));
4996 auto res = tree.segmentQuery(ro, re, delegate (flesh, in ref vec2 a, in ref vec2 b) {
4997 auto dst = flesh.aabb.segIntersectMin(a, b);
4998 writeln("a=", a, "; b=", b, "; dst=", dst);
4999 if (dst < 0) return -1;