1 | /******************************************************************************* |
2 | Copyright (c) The Taichi Authors (2016- ). All Rights Reserved. |
3 | The use of this software is governed by the LICENSE file. |
4 | *******************************************************************************/ |
5 | |
6 | #pragma once |
7 | |
8 | #include <cmath> |
9 | #include <type_traits> |
10 | #include <functional> |
11 | #include <vector> |
12 | #include <array> |
13 | #include "taichi/common/core.h" |
14 | #include "scalar.h" |
15 | #include "array_fwd.h" |
16 | namespace taichi { |
17 | |
18 | // Instruction Set Extension |
19 | |
20 | enum class InstSetExt { None }; |
21 | |
22 | constexpr InstSetExt default_instruction_set = InstSetExt::None; |
23 | |
24 | ///////////////////////////////////////////////////////////////// |
25 | ///// N dimensional Vector |
26 | ///////////////////////////////////////////////////////////////// |
27 | |
28 | template <int dim, typename T, InstSetExt ISE> |
29 | struct VectorNDBase { |
30 | static constexpr bool simd = false; |
31 | static constexpr int storage_elements = dim; |
32 | T d[dim]; |
33 | }; |
34 | |
35 | template <typename T, InstSetExt ISE> |
36 | struct VectorNDBase<1, T, ISE> { |
37 | static constexpr bool simd = false; |
38 | static constexpr int storage_elements = 1; |
39 | union { |
40 | T d[1]; |
41 | struct { |
42 | T x; |
43 | }; |
44 | }; |
45 | }; |
46 | |
47 | template <typename T, InstSetExt ISE> |
48 | struct VectorNDBase<2, T, ISE> { |
49 | static constexpr bool simd = false; |
50 | static constexpr int storage_elements = 2; |
51 | union { |
52 | T d[2]; |
53 | struct { |
54 | T x, y; |
55 | }; |
56 | }; |
57 | }; |
58 | |
59 | template <typename T, InstSetExt ISE> |
60 | struct VectorNDBase<3, T, ISE> { |
61 | static constexpr bool simd = false; |
62 | static constexpr int storage_elements = 3; |
63 | union { |
64 | T d[3]; |
65 | struct { |
66 | T x, y, z; |
67 | }; |
68 | }; |
69 | }; |
70 | |
71 | template <typename T, InstSetExt ISE> |
72 | struct VectorNDBase<4, T, ISE> { |
73 | static constexpr int storage_elements = 4; |
74 | static constexpr bool simd = false; |
75 | union { |
76 | T d[4]; |
77 | struct { |
78 | T x, y, z, w; |
79 | }; |
80 | }; |
81 | }; |
82 | |
83 | template <int dim__, typename T, InstSetExt ISE = default_instruction_set> |
84 | struct VectorND : public VectorNDBase<dim__, T, ISE> { |
85 | static constexpr int dim = dim__; |
86 | using ScalarType = T; |
87 | |
88 | using type = T; |
89 | |
90 | using VectorBase = VectorNDBase<dim, T, ISE>; |
91 | using VectorBase::d; |
92 | static constexpr int storage_elements = VectorBase::storage_elements; |
93 | |
94 | TI_FORCE_INLINE VectorND() { |
95 | for (int i = 0; i < dim; i++) { |
96 | this->d[i] = T(0); |
97 | } |
98 | } |
99 | |
100 | static TI_FORCE_INLINE VectorND from_array(const T new_val[dim]) { |
101 | VectorND ret; |
102 | for (int i = 0; i < dim; i++) { |
103 | ret.d[i] = new_val[i]; |
104 | } |
105 | return ret; |
106 | } |
107 | |
108 | template <int dim_, typename T_, InstSetExt ISE_> |
109 | explicit TI_FORCE_INLINE VectorND(const VectorND<dim_, T_, ISE_> &o) |
110 | : VectorND() { |
111 | for (int i = 0; i < std::min(dim_, dim__); i++) { |
112 | d[i] = o[i]; |
113 | } |
114 | } |
115 | |
116 | explicit TI_FORCE_INLINE VectorND(const std::array<T, dim> &o) { |
117 | for (int i = 0; i < dim; i++) { |
118 | d[i] = o[i]; |
119 | } |
120 | } |
121 | |
122 | template <typename T_ = T, |
123 | typename std::enable_if_t<std::is_same<T_, int>::value, int> = 0> |
124 | explicit VectorND(const TIndex<dim> &ind); |
125 | |
126 | // Vector initialization |
127 | template <typename F, |
128 | std::enable_if_t<std::is_same<F, VectorND>::value, int> = 0> |
129 | explicit TI_FORCE_INLINE VectorND(const F &f) { |
130 | for (int i = 0; i < dim; i++) |
131 | this->d[i] = f[i]; |
132 | } |
133 | |
134 | // Scalar initialization |
135 | template <typename F, std::enable_if_t<std::is_same<F, T>::value, int> = 0> |
136 | explicit TI_FORCE_INLINE VectorND(const F &f) { |
137 | for (int i = 0; i < dim; i++) |
138 | this->d[i] = f; |
139 | } |
140 | |
141 | // Function initialization |
142 | template < |
143 | typename F, |
144 | std::enable_if_t<std::is_convertible<F, std::function<T(int)>>::value, |
145 | int> = 0> |
146 | explicit TI_FORCE_INLINE VectorND(const F &f) { |
147 | for (int i = 0; i < dim; i++) |
148 | this->d[i] = f(i); |
149 | } |
150 | |
151 | template <int dim_ = dim, typename T_ = T, InstSetExt ISE_ = ISE> |
152 | explicit TI_FORCE_INLINE VectorND(T v) { |
153 | for (int i = 0; i < dim; i++) { |
154 | this->d[i] = v; |
155 | } |
156 | } |
157 | |
158 | explicit TI_FORCE_INLINE VectorND(T v0, T v1) { |
159 | static_assert(dim == 2, "Vector dim must be 2" ); |
160 | this->d[0] = v0; |
161 | this->d[1] = v1; |
162 | } |
163 | |
164 | // All except Vector3f |
165 | template <int dim_ = dim, typename T_ = T, InstSetExt ISE_ = ISE> |
166 | explicit TI_FORCE_INLINE VectorND(T v0, T v1, T v2) { |
167 | static_assert(dim == 3, "Vector dim must be 3" ); |
168 | this->d[0] = v0; |
169 | this->d[1] = v1; |
170 | this->d[2] = v2; |
171 | } |
172 | |
173 | // All except Vector3f, Vector4f |
174 | template <int dim_ = dim, typename T_ = T, InstSetExt ISE_ = ISE> |
175 | explicit TI_FORCE_INLINE VectorND(T v0, T v1, T v2, T v3) { |
176 | static_assert(dim == 4, "Vector dim must be 4" ); |
177 | this->d[0] = v0; |
178 | this->d[1] = v1; |
179 | this->d[2] = v2; |
180 | this->d[3] = v3; |
181 | } |
182 | |
183 | // Vector extension |
184 | template <int dim_ = dim, std::enable_if_t<(dim_ > 1), int> = 0> |
185 | explicit TI_FORCE_INLINE VectorND(const VectorND<dim - 1, T, ISE> &o, |
186 | T ) { |
187 | for (int i = 0; i < dim_ - 1; i++) { |
188 | this->d[i] = o[i]; |
189 | } |
190 | this->d[dim - 1] = extra; |
191 | } |
192 | |
193 | template <typename T_> |
194 | explicit TI_FORCE_INLINE VectorND(const std::vector<T_> &o) { |
195 | if (o.size() != dim) { |
196 | TI_ERROR("Dimension mismatch: " + std::to_string(dim) + " v.s. " + |
197 | std::to_string((int)o.size())); |
198 | } |
199 | for (int i = 0; i < dim; i++) |
200 | this->d[i] = T(o[i]); |
201 | } |
202 | |
203 | TI_FORCE_INLINE T &operator[](int i) { |
204 | return this->d[i]; |
205 | } |
206 | |
207 | TI_FORCE_INLINE const T &operator[](int i) const { |
208 | return this->d[i]; |
209 | } |
210 | |
211 | TI_FORCE_INLINE T &operator()(int i) { |
212 | return d[i]; |
213 | } |
214 | |
215 | TI_FORCE_INLINE const T &operator()(int i) const { |
216 | return d[i]; |
217 | } |
218 | |
219 | TI_FORCE_INLINE T dot(VectorND<dim, T, ISE> o) const { |
220 | T ret = T(0); |
221 | for (int i = 0; i < dim; i++) |
222 | ret += this->d[i] * o[i]; |
223 | return ret; |
224 | } |
225 | |
226 | template < |
227 | typename F, |
228 | std::enable_if_t<std::is_convertible<F, std::function<T(int)>>::value, |
229 | int> = 0> |
230 | TI_FORCE_INLINE VectorND &set(const F &f) { |
231 | for (int i = 0; i < dim; i++) |
232 | this->d[i] = f(i); |
233 | return *this; |
234 | } |
235 | |
236 | TI_FORCE_INLINE auto map(T(f)(T)) const |
237 | -> VectorND<dim, decltype(f(T(0))), ISE> { |
238 | VectorND<dim, decltype(f(T(0))), ISE> ret; |
239 | for (int i = 0; i < dim; i++) |
240 | ret[i] = f(this->d[i]); |
241 | return ret; |
242 | } |
243 | |
244 | TI_FORCE_INLINE VectorND &operator=(const VectorND &o) { |
245 | memcpy(this, &o, sizeof(*this)); |
246 | return *this; |
247 | } |
248 | |
249 | // Non-SIMD cases |
250 | template <int dim_ = dim, typename T_ = T, InstSetExt ISE_ = ISE> |
251 | TI_FORCE_INLINE VectorND operator+(const VectorND &o) const { |
252 | return VectorND([=](int i) { return this->d[i] + o[i]; }); |
253 | } |
254 | |
255 | template <int dim_ = dim, typename T_ = T, InstSetExt ISE_ = ISE> |
256 | TI_FORCE_INLINE VectorND operator-(const VectorND &o) const { |
257 | return VectorND([=](int i) { return this->d[i] - o[i]; }); |
258 | } |
259 | |
260 | template <int dim_ = dim, typename T_ = T, InstSetExt ISE_ = ISE> |
261 | TI_FORCE_INLINE VectorND operator*(const VectorND &o) const { |
262 | return VectorND([=](int i) { return this->d[i] * o[i]; }); |
263 | } |
264 | |
265 | template <int dim_ = dim, typename T_ = T, InstSetExt ISE_ = ISE> |
266 | TI_FORCE_INLINE VectorND operator/(const VectorND &o) const { |
267 | return VectorND([=](int i) { return this->d[i] / o[i]; }); |
268 | } |
269 | |
270 | template <int dim_ = dim, typename T_ = T, InstSetExt ISE_ = ISE> |
271 | TI_FORCE_INLINE VectorND operator%(const VectorND &o) const { |
272 | return VectorND([=](int i) { return this->d[i] % o[i]; }); |
273 | } |
274 | |
275 | // Inplace operations |
276 | TI_FORCE_INLINE VectorND &operator+=(const VectorND &o) { |
277 | (*this) = (*this) + o; |
278 | return *this; |
279 | } |
280 | |
281 | TI_FORCE_INLINE VectorND &operator-=(const VectorND &o) { |
282 | (*this) = (*this) - o; |
283 | return *this; |
284 | } |
285 | |
286 | TI_FORCE_INLINE VectorND &operator*=(const VectorND &o) { |
287 | (*this) = (*this) * o; |
288 | return *this; |
289 | } |
290 | |
291 | TI_FORCE_INLINE VectorND &operator*=(const T &o) { |
292 | (*this) = (*this) * o; |
293 | return *this; |
294 | } |
295 | |
296 | TI_FORCE_INLINE VectorND &operator/=(const VectorND &o) { |
297 | (*this) = (*this) / o; |
298 | return *this; |
299 | } |
300 | |
301 | TI_FORCE_INLINE VectorND &operator/=(const T &o) { |
302 | (*this) = (*this) / o; |
303 | return *this; |
304 | } |
305 | |
306 | TI_FORCE_INLINE VectorND operator-() const { |
307 | return VectorND([=](int i) { return -this->d[i]; }); |
308 | } |
309 | |
310 | TI_FORCE_INLINE bool operator==(const VectorND &o) const { |
311 | for (int i = 0; i < dim; i++) |
312 | if (this->d[i] != o[i]) |
313 | return false; |
314 | return true; |
315 | } |
316 | |
317 | TI_FORCE_INLINE bool operator<(const VectorND &o) const { |
318 | for (int i = 0; i < dim; i++) |
319 | if (this->d[i] >= o[i]) |
320 | return false; |
321 | return true; |
322 | } |
323 | |
324 | TI_FORCE_INLINE bool operator<=(const VectorND &o) const { |
325 | for (int i = 0; i < dim; i++) |
326 | if (this->d[i] > o[i]) |
327 | return false; |
328 | return true; |
329 | } |
330 | |
331 | TI_FORCE_INLINE bool operator>(const VectorND &o) const { |
332 | for (int i = 0; i < dim; i++) |
333 | if (this->d[i] <= o[i]) |
334 | return false; |
335 | return true; |
336 | } |
337 | |
338 | TI_FORCE_INLINE bool operator>=(const VectorND &o) const { |
339 | for (int i = 0; i < dim; i++) |
340 | if (this->d[i] < o[i]) |
341 | return false; |
342 | return true; |
343 | } |
344 | |
345 | TI_FORCE_INLINE bool operator==(const std::vector<T> &o) const { |
346 | if (o.size() != dim) |
347 | return false; |
348 | for (int i = 0; i < dim; i++) |
349 | if (this->d[i] != o[i]) |
350 | return false; |
351 | return true; |
352 | } |
353 | |
354 | TI_FORCE_INLINE bool operator!=(const VectorND &o) const { |
355 | for (int i = 0; i < dim; i++) |
356 | if (this->d[i] != o[i]) |
357 | return true; |
358 | return false; |
359 | } |
360 | |
361 | TI_FORCE_INLINE VectorND abs() const { |
362 | return VectorND([&](int i) { return std::abs(d[i]); }); |
363 | } |
364 | |
365 | TI_FORCE_INLINE VectorND floor() const { |
366 | return VectorND([&](int i) { return std::floor(d[i]); }); |
367 | } |
368 | |
369 | TI_FORCE_INLINE VectorND sin() const { |
370 | return VectorND([&](int i) { return std::sin(d[i]); }); |
371 | } |
372 | |
373 | TI_FORCE_INLINE VectorND cos() const { |
374 | return VectorND([&](int i) { return std::cos(d[i]); }); |
375 | } |
376 | |
377 | TI_FORCE_INLINE VectorND fract() const { |
378 | return VectorND([&](int i) { return taichi::fract(d[i]); }); |
379 | } |
380 | |
381 | TI_FORCE_INLINE VectorND clamp() const { |
382 | return VectorND([&](int i) { return taichi::clamp(d[i]); }); |
383 | } |
384 | |
385 | TI_FORCE_INLINE VectorND clamp(const T &a, T &b) const { |
386 | return VectorND([&](int i) { return taichi::clamp(d[i], a, b); }); |
387 | } |
388 | |
389 | TI_FORCE_INLINE VectorND clamp(const VectorND &a, const VectorND &b) const { |
390 | return VectorND([&](int i) { return taichi::clamp(d[i], a[i], b[i]); }); |
391 | } |
392 | |
393 | TI_FORCE_INLINE T min() const { |
394 | T ret = this->d[0]; |
395 | for (int i = 1; i < dim; i++) { |
396 | ret = std::min(ret, this->d[i]); |
397 | } |
398 | return ret; |
399 | } |
400 | |
401 | TI_FORCE_INLINE T max() const { |
402 | T ret = this->d[0]; |
403 | for (int i = 1; i < dim; i++) { |
404 | ret = std::max(ret, this->d[i]); |
405 | } |
406 | return ret; |
407 | } |
408 | |
409 | TI_FORCE_INLINE T abs_max() const { |
410 | T ret = std::abs(this->d[0]); |
411 | for (int i = 1; i < dim; i++) { |
412 | ret = std::max(ret, std::abs(this->d[i])); |
413 | } |
414 | return ret; |
415 | } |
416 | |
417 | template <typename G> |
418 | TI_FORCE_INLINE VectorND<dim, G, ISE> cast() const { |
419 | return VectorND<dim, G, ISE>( |
420 | [this](int i) { return static_cast<G>(this->d[i]); }); |
421 | } |
422 | |
423 | void print() const { |
424 | for (int i = 0; i < dim; i++) { |
425 | std::cout << this->d[i] << " " ; |
426 | } |
427 | std::cout << std::endl; |
428 | } |
429 | |
430 | template <int a, |
431 | int b, |
432 | int c, |
433 | int d, |
434 | int dim_ = dim, |
435 | typename T_ = T, |
436 | InstSetExt ISE_ = ISE> |
437 | TI_FORCE_INLINE VectorND permute() const { |
438 | return VectorND(this->d[a], this->d[b], this->d[c], this->d[d]); |
439 | } |
440 | |
441 | template <int a, int dim_ = dim, typename T_ = T, InstSetExt ISE_ = ISE> |
442 | TI_FORCE_INLINE VectorND broadcast() const { |
443 | return permute<a, a, a, a>(); |
444 | } |
445 | |
446 | template <int dim_ = dim, typename T_ = T, InstSetExt ISE_ = ISE> |
447 | TI_FORCE_INLINE T length2() const { |
448 | T ret = 0; |
449 | for (int i = 0; i < dim; i++) { |
450 | ret += this->d[i] * this->d[i]; |
451 | } |
452 | return ret; |
453 | } |
454 | |
455 | TI_FORCE_INLINE auto length() const { |
456 | return std::sqrt(length2()); |
457 | } |
458 | |
459 | bool is_normal() const { |
460 | for (int i = 0; i < dim; i++) { |
461 | if (!taichi::is_normal(this->d[i])) |
462 | return false; |
463 | } |
464 | return true; |
465 | } |
466 | |
467 | bool abnormal() const { |
468 | return !this->is_normal(); |
469 | } |
470 | |
471 | static VectorND rand() { |
472 | VectorND ret; |
473 | for (int i = 0; i < dim; i++) { |
474 | ret[i] = taichi::rand(); |
475 | } |
476 | return ret; |
477 | } |
478 | |
479 | TI_FORCE_INLINE T sum() const { |
480 | T ret = this->d[0]; |
481 | for (int i = 1; i < dim; i++) { |
482 | ret += this->d[i]; |
483 | } |
484 | return ret; |
485 | } |
486 | |
487 | TI_FORCE_INLINE T average() const { |
488 | return (T(1.0) / dim) * sum(); |
489 | } |
490 | |
491 | TI_FORCE_INLINE T prod() const { |
492 | T ret = this->d[0]; |
493 | for (int i = 1; i < dim; i++) { |
494 | ret *= this->d[i]; |
495 | } |
496 | return ret; |
497 | } |
498 | |
499 | TI_FORCE_INLINE VectorND pow(T index) const { |
500 | VectorND ret; |
501 | for (int i = 0; i < dim; i++) { |
502 | ret[i] = std::pow(this->d[i], index); |
503 | } |
504 | return ret; |
505 | } |
506 | |
507 | TI_FORCE_INLINE static VectorND axis(int i) { |
508 | VectorND ret(0); |
509 | ret[i] = 1; |
510 | return ret; |
511 | } |
512 | |
513 | TI_IO_DEF(d); |
514 | |
515 | TI_FORCE_INLINE explicit operator std::array<T, dim>() const { |
516 | std::array<T, dim> arr; |
517 | for (int i = 0; i < dim; i++) { |
518 | arr[i] = d[i]; |
519 | } |
520 | return arr; |
521 | } |
522 | }; |
523 | |
524 | template <typename T, int dim, InstSetExt ISE = default_instruction_set> |
525 | using TVector = VectorND<dim, T, ISE>; |
526 | |
527 | template <int dim, typename T, InstSetExt ISE> |
528 | TI_FORCE_INLINE VectorND<dim, T, ISE> operator*( |
529 | T a, |
530 | const VectorND<dim, T, ISE> &v) { |
531 | return VectorND<dim, T, ISE>(a) * v; |
532 | } |
533 | |
534 | template <int dim, typename T, InstSetExt ISE> |
535 | TI_FORCE_INLINE VectorND<dim, T, ISE> operator*(const VectorND<dim, T, ISE> &v, |
536 | T a) { |
537 | return a * v; |
538 | } |
539 | |
540 | template <int dim, typename T, InstSetExt ISE> |
541 | TI_FORCE_INLINE VectorND<dim, T, ISE> operator/( |
542 | T a, |
543 | const VectorND<dim, T, ISE> &v) { |
544 | return VectorND<dim, T, ISE>(a) / v; |
545 | } |
546 | |
547 | template <int dim, typename T, InstSetExt ISE> |
548 | TI_FORCE_INLINE VectorND<dim, T, ISE> operator/(const VectorND<dim, T, ISE> &v, |
549 | T a) { |
550 | return v / VectorND<dim, T, ISE>(a); |
551 | } |
552 | |
553 | template <typename T> |
554 | TI_FORCE_INLINE std::array<T, 1> to_std_array(const TVector<T, 1> &v) { |
555 | return std::array<T, 1>{v[0]}; |
556 | } |
557 | |
558 | template <typename T> |
559 | TI_FORCE_INLINE std::array<T, 2> to_std_array(const TVector<T, 2> &v) { |
560 | return std::array<T, 2>{v[0], v[1]}; |
561 | } |
562 | |
563 | template <typename T> |
564 | TI_FORCE_INLINE std::array<T, 3> to_std_array(const TVector<T, 3> &v) { |
565 | return std::array<T, 3>{v[0], v[1], v[2]}; |
566 | } |
567 | |
568 | template <typename T> |
569 | TI_FORCE_INLINE std::array<T, 4> to_std_array(const TVector<T, 4> &v) { |
570 | return std::array<T, 4>{v[0], v[1], v[2], v[3]}; |
571 | } |
572 | |
573 | using Vector1 = VectorND<1, real, default_instruction_set>; |
574 | using Vector2 = VectorND<2, real, default_instruction_set>; |
575 | using Vector3 = VectorND<3, real, default_instruction_set>; |
576 | using Vector4 = VectorND<4, real, default_instruction_set>; |
577 | |
578 | using Vector1f = VectorND<1, float32, default_instruction_set>; |
579 | using Vector2f = VectorND<2, float32, default_instruction_set>; |
580 | using Vector3f = VectorND<3, float32, default_instruction_set>; |
581 | using Vector4f = VectorND<4, float32, default_instruction_set>; |
582 | |
583 | using Vector1d = VectorND<1, float64, default_instruction_set>; |
584 | using Vector2d = VectorND<2, float64, default_instruction_set>; |
585 | using Vector3d = VectorND<3, float64, default_instruction_set>; |
586 | using Vector4d = VectorND<4, float64, default_instruction_set>; |
587 | |
588 | using Vector1i = VectorND<1, int, default_instruction_set>; |
589 | using Vector2i = VectorND<2, int, default_instruction_set>; |
590 | using Vector3i = VectorND<3, int, default_instruction_set>; |
591 | using Vector4i = VectorND<4, int, default_instruction_set>; |
592 | |
593 | template <typename T> |
594 | TI_FORCE_INLINE T fused_mul_add(const T &a, const T &b, const T &c) { |
595 | return a * b + c; |
596 | } |
597 | |
598 | ///////////////////////////////////////////////////////////////// |
599 | ///// N dimensional Matrix |
600 | ///////////////////////////////////////////////////////////////// |
601 | |
602 | template <int dim__, typename T, InstSetExt ISE = default_instruction_set> |
603 | struct MatrixND { |
604 | static constexpr int dim = dim__; |
605 | |
606 | using ScalarType = T; |
607 | |
608 | using Vector = VectorND<dim, T, ISE>; |
609 | Vector d[dim]; |
610 | |
611 | static constexpr InstSetExt ise = ISE; |
612 | using type = T; |
613 | |
614 | TI_FORCE_INLINE MatrixND() { |
615 | for (int i = 0; i < dim; i++) { |
616 | d[i] = VectorND<dim, T, ISE>(); |
617 | } |
618 | } |
619 | |
620 | template <int dim_, typename T_, InstSetExt ISE_> |
621 | TI_FORCE_INLINE explicit MatrixND(const MatrixND<dim_, T_, ISE_> &o) |
622 | : MatrixND() { |
623 | for (int i = 0; i < std::min(dim_, dim__); i++) { |
624 | for (int j = 0; j < std::min(dim_, dim__); j++) { |
625 | d[i][j] = o[i][j]; |
626 | } |
627 | } |
628 | } |
629 | |
630 | TI_FORCE_INLINE explicit MatrixND(T v) : MatrixND() { |
631 | for (int i = 0; i < dim; i++) { |
632 | d[i][i] = v; |
633 | } |
634 | } |
635 | |
636 | TI_FORCE_INLINE MatrixND(const MatrixND &o) { |
637 | *this = o; |
638 | } |
639 | |
640 | // Diag |
641 | TI_FORCE_INLINE explicit MatrixND(Vector v) : MatrixND() { |
642 | for (int i = 0; i < dim; i++) |
643 | this->d[i][i] = v[i]; |
644 | } |
645 | |
646 | TI_FORCE_INLINE explicit MatrixND(Vector v0, Vector v1) { |
647 | static_assert(dim == 2, "Matrix dim must be 2" ); |
648 | this->d[0] = v0; |
649 | this->d[1] = v1; |
650 | } |
651 | |
652 | TI_FORCE_INLINE explicit MatrixND(Vector v0, Vector v1, Vector v2) { |
653 | static_assert(dim == 3, "Matrix dim must be 3" ); |
654 | this->d[0] = v0; |
655 | this->d[1] = v1; |
656 | this->d[2] = v2; |
657 | } |
658 | |
659 | TI_FORCE_INLINE explicit MatrixND(Vector v0, |
660 | Vector v1, |
661 | Vector v2, |
662 | Vector v3) { |
663 | static_assert(dim == 4, "Matrix dim must be 4" ); |
664 | this->d[0] = v0; |
665 | this->d[1] = v1; |
666 | this->d[2] = v2; |
667 | this->d[3] = v3; |
668 | } |
669 | |
670 | // Function initialization |
671 | template < |
672 | typename F, |
673 | std::enable_if_t<std::is_convertible< |
674 | F, |
675 | std::function<VectorND<dim__, T, ISE>(int)>>::value, |
676 | int> = 0> |
677 | TI_FORCE_INLINE explicit MatrixND(const F &f) { |
678 | for (int i = 0; i < dim; i++) |
679 | this->d[i] = f(i); |
680 | } |
681 | |
682 | template < |
683 | typename F, |
684 | std::enable_if_t<std::is_convertible< |
685 | F, |
686 | std::function<VectorND<dim__, T, ISE>(int)>>::value, |
687 | int> = 0> |
688 | TI_FORCE_INLINE MatrixND &set(const F &f) { |
689 | for (int i = 0; i < dim; i++) |
690 | this->d[i] = f(i); |
691 | return *this; |
692 | } |
693 | |
694 | TI_FORCE_INLINE MatrixND &operator=(const MatrixND &o) { |
695 | for (int i = 0; i < dim; i++) { |
696 | this->d[i] = o[i]; |
697 | } |
698 | return *this; |
699 | } |
700 | |
701 | TI_FORCE_INLINE VectorND<dim, T, ISE> &operator[](int i) { |
702 | return d[i]; |
703 | } |
704 | |
705 | TI_FORCE_INLINE T &operator()(int i, int j) { |
706 | return d[j][i]; |
707 | } |
708 | |
709 | TI_FORCE_INLINE const T &operator()(int i, int j) const { |
710 | return d[j][i]; |
711 | } |
712 | |
713 | TI_FORCE_INLINE const VectorND<dim, T, ISE> &operator[](int i) const { |
714 | return d[i]; |
715 | } |
716 | |
717 | template <int dim_ = dim, typename T_ = T, InstSetExt ISE_ = ISE> |
718 | TI_FORCE_INLINE VectorND<dim, T, ISE> operator*( |
719 | const VectorND<dim, T, ISE> &o) const { |
720 | VectorND<dim, T, ISE> ret = d[0] * o[0]; |
721 | for (int i = 1; i < dim; i++) |
722 | ret += d[i] * o[i]; |
723 | return ret; |
724 | } |
725 | |
726 | template <int dim_ = dim, typename T_ = T, InstSetExt ISE_ = ISE> |
727 | TI_FORCE_INLINE MatrixND operator*(const MatrixND &o) const { |
728 | MatrixND ret; |
729 | for (int i = 0; i < dim; i++) { |
730 | for (int j = 0; j < dim; j++) { |
731 | T tmp = 0; |
732 | for (int k = 0; k < dim; k++) { |
733 | tmp += (*this)[k][j] * o[i][k]; |
734 | } |
735 | ret[i][j] = tmp; |
736 | } |
737 | } |
738 | return ret; |
739 | } |
740 | |
741 | TI_FORCE_INLINE static MatrixND outer_product(Vector column, Vector row) { |
742 | return MatrixND([&](int i) { return column * row[i]; }); |
743 | } |
744 | |
745 | TI_FORCE_INLINE MatrixND operator+(const MatrixND &o) const { |
746 | return MatrixND([=](int i) { return this->d[i] + o[i]; }); |
747 | } |
748 | |
749 | TI_FORCE_INLINE MatrixND operator-(const MatrixND &o) const { |
750 | return MatrixND([=](int i) { return this->d[i] - o[i]; }); |
751 | } |
752 | |
753 | TI_FORCE_INLINE MatrixND &operator+=(const MatrixND &o) { |
754 | return this->set([&](int i) { return this->d[i] + o[i]; }); |
755 | } |
756 | |
757 | TI_FORCE_INLINE MatrixND &operator-=(const MatrixND &o) { |
758 | return this->set([&](int i) { return this->d[i] - o[i]; }); |
759 | } |
760 | |
761 | TI_FORCE_INLINE MatrixND operator-() const { |
762 | return MatrixND([=](int i) { return -this->d[i]; }); |
763 | } |
764 | |
765 | TI_FORCE_INLINE bool operator==(const MatrixND &o) const { |
766 | for (int i = 0; i < dim; i++) |
767 | for (int j = 0; j < dim; j++) |
768 | if (d[i][j] != o[i][j]) |
769 | return false; |
770 | return true; |
771 | } |
772 | |
773 | TI_FORCE_INLINE bool operator!=(const MatrixND &o) const { |
774 | for (int i = 0; i < dim; i++) |
775 | for (int j = 0; j < dim; j++) |
776 | if (d[i][j] != o[i][j]) |
777 | return true; |
778 | return false; |
779 | } |
780 | |
781 | TI_FORCE_INLINE T frobenius_norm2() const { |
782 | T sum = d[0].length2(); |
783 | for (int i = 1; i < dim; i++) { |
784 | sum += d[i].length2(); |
785 | } |
786 | return sum; |
787 | } |
788 | |
789 | TI_FORCE_INLINE auto frobenius_norm() const { |
790 | return std::sqrt(frobenius_norm2()); |
791 | } |
792 | |
793 | TI_FORCE_INLINE MatrixND transposed() const { |
794 | MatrixND ret; |
795 | for (int i = 0; i < dim; i++) { |
796 | for (int j = 0; j < dim; j++) { |
797 | ret[i][j] = d[j][i]; |
798 | } |
799 | } |
800 | return ret; |
801 | } |
802 | |
803 | template <typename G> |
804 | TI_FORCE_INLINE MatrixND<dim, G, ISE> cast() const { |
805 | return MatrixND<dim, G, ISE>( |
806 | [=](int i) { return d[i].template cast<G>(); }); |
807 | } |
808 | |
809 | bool is_normal() const { |
810 | for (int i = 0; i < dim; i++) { |
811 | if (!this->d[i].is_normal()) |
812 | return false; |
813 | } |
814 | return true; |
815 | } |
816 | |
817 | bool abnormal() const { |
818 | return !this->is_normal(); |
819 | } |
820 | |
821 | static MatrixND rand() { |
822 | MatrixND ret; |
823 | for (int i = 0; i < dim; i++) { |
824 | ret[i] = Vector::rand(); |
825 | } |
826 | return ret; |
827 | } |
828 | |
829 | TI_FORCE_INLINE Vector diag() const { |
830 | Vector ret; |
831 | for (int i = 0; i < dim; i++) { |
832 | ret[i] = this->d[i][i]; |
833 | } |
834 | return ret; |
835 | } |
836 | |
837 | TI_FORCE_INLINE T sum() const { |
838 | T ret(0); |
839 | for (int i = 0; i < dim; i++) { |
840 | ret += this->d[i].sum(); |
841 | } |
842 | return ret; |
843 | } |
844 | |
845 | TI_FORCE_INLINE T trace() const { |
846 | return this->diag().sum(); |
847 | } |
848 | |
849 | TI_FORCE_INLINE T tr() const { |
850 | return this->trace(); |
851 | } |
852 | |
853 | TI_FORCE_INLINE MatrixND |
854 | elementwise_product(const MatrixND<dim, T> &o) const { |
855 | MatrixND ret; |
856 | for (int i = 0; i < dim; i++) { |
857 | ret[i] = this->d[i] * o[i]; |
858 | } |
859 | return ret; |
860 | } |
861 | |
862 | TI_FORCE_INLINE static MatrixND identidy() { |
863 | return MatrixND(1.0_f); |
864 | } |
865 | |
866 | TI_IO_DEF(d); |
867 | }; |
868 | |
869 | template <int dim, typename T, InstSetExt ISE> |
870 | TI_FORCE_INLINE MatrixND<dim, T, ISE> operator*( |
871 | const T a, |
872 | const MatrixND<dim, T, ISE> &M) { |
873 | MatrixND<dim, T, ISE> ret; |
874 | for (int i = 0; i < dim; i++) { |
875 | ret[i] = a * M[i]; |
876 | } |
877 | return ret; |
878 | } |
879 | |
880 | template <int dim, typename T, InstSetExt ISE> |
881 | TI_FORCE_INLINE MatrixND<dim, T, ISE> operator*(const MatrixND<dim, T, ISE> &M, |
882 | const T a) { |
883 | return a * M; |
884 | } |
885 | |
886 | template <int dim, typename T, InstSetExt ISE> |
887 | TI_FORCE_INLINE MatrixND<dim, T, ISE> transpose( |
888 | const MatrixND<dim, T, ISE> &mat) { |
889 | return mat.transposed(); |
890 | } |
891 | |
892 | template <int dim, typename T, InstSetExt ISE> |
893 | TI_FORCE_INLINE MatrixND<dim, T, ISE> transposed( |
894 | const MatrixND<dim, T, ISE> &mat) { |
895 | return transpose(mat); |
896 | } |
897 | |
898 | template <typename T, int dim, InstSetExt ISE = default_instruction_set> |
899 | using TMatrix = MatrixND<dim, T, ISE>; |
900 | |
901 | using Matrix2 = MatrixND<2, real, default_instruction_set>; |
902 | using Matrix3 = MatrixND<3, real, default_instruction_set>; |
903 | using Matrix4 = MatrixND<4, real, default_instruction_set>; |
904 | |
905 | using Matrix2f = MatrixND<2, float32, default_instruction_set>; |
906 | using Matrix3f = MatrixND<3, float32, default_instruction_set>; |
907 | using Matrix4f = MatrixND<4, float32, default_instruction_set>; |
908 | |
909 | using Matrix2d = MatrixND<2, float64, default_instruction_set>; |
910 | using Matrix3d = MatrixND<3, float64, default_instruction_set>; |
911 | using Matrix4d = MatrixND<4, float64, default_instruction_set>; |
912 | |
913 | template <typename T, InstSetExt ISE> |
914 | TI_FORCE_INLINE real determinant(const MatrixND<2, T, ISE> &mat) { |
915 | return mat[0][0] * mat[1][1] - mat[0][1] * mat[1][0]; |
916 | } |
917 | |
918 | template <typename T, InstSetExt ISE> |
919 | TI_FORCE_INLINE T determinant(const MatrixND<3, T, ISE> &mat) { |
920 | return mat[0][0] * (mat[1][1] * mat[2][2] - mat[2][1] * mat[1][2]) - |
921 | mat[1][0] * (mat[0][1] * mat[2][2] - mat[2][1] * mat[0][2]) + |
922 | mat[2][0] * (mat[0][1] * mat[1][2] - mat[1][1] * mat[0][2]); |
923 | } |
924 | |
925 | template <typename T, InstSetExt ISE> |
926 | TI_FORCE_INLINE T cross(const VectorND<2, T, ISE> &a, |
927 | const VectorND<2, T, ISE> &b) { |
928 | return a.x * b.y - a.y * b.x; |
929 | } |
930 | |
931 | template <typename T, InstSetExt ISE> |
932 | TI_FORCE_INLINE VectorND<3, T, ISE> cross(const VectorND<3, T, ISE> &a, |
933 | const VectorND<3, T, ISE> &b) { |
934 | return VectorND<3, T, ISE>(a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, |
935 | a.x * b.y - a.y * b.x); |
936 | } |
937 | |
938 | template <int dim, typename T, InstSetExt ISE> |
939 | TI_FORCE_INLINE T dot(const VectorND<dim, T, ISE> &a, |
940 | const VectorND<dim, T, ISE> &b) { |
941 | return a.dot(b); |
942 | } |
943 | |
944 | template <int dim, typename T, InstSetExt ISE> |
945 | TI_FORCE_INLINE VectorND<dim, T, ISE> normalize( |
946 | const VectorND<dim, T, ISE> &a) { |
947 | return (T(1) / a.length()) * a; |
948 | } |
949 | |
950 | template <int dim, typename T, InstSetExt ISE> |
951 | TI_FORCE_INLINE VectorND<dim, T, ISE> normalized( |
952 | const VectorND<dim, T, ISE> &a) { |
953 | return normalize(a); |
954 | } |
955 | |
956 | TI_FORCE_INLINE float32 length(const float32 &a) { |
957 | return a; |
958 | } |
959 | |
960 | TI_FORCE_INLINE float64 length(const float64 &a) { |
961 | return a; |
962 | } |
963 | |
964 | template <int dim, typename T, InstSetExt ISE> |
965 | TI_FORCE_INLINE T length(const VectorND<dim, T, ISE> &a) { |
966 | return a.length(); |
967 | } |
968 | |
969 | template <int dim, typename T, InstSetExt ISE> |
970 | TI_FORCE_INLINE T length2(const VectorND<dim, T, ISE> &a) { |
971 | return dot(a, a); |
972 | } |
973 | |
974 | TI_FORCE_INLINE float32 length2(const float32 &a) { |
975 | return a * a; |
976 | } |
977 | |
978 | TI_FORCE_INLINE float64 length2(const float64 &a) { |
979 | return a * a; |
980 | } |
981 | |
982 | template <int dim, typename T, InstSetExt ISE> |
983 | TI_FORCE_INLINE VectorND<dim, T, ISE> fract(const VectorND<dim, T, ISE> &a) { |
984 | return a.fract(); |
985 | } |
986 | |
987 | TI_FORCE_INLINE float32 inversed(const float32 &a) { |
988 | return 1.0_f32 / a; |
989 | } |
990 | |
991 | TI_FORCE_INLINE float64 inversed(const float64 &a) { |
992 | return 1.0_f64 / a; |
993 | } |
994 | |
995 | template <InstSetExt ISE, typename T> |
996 | TI_FORCE_INLINE MatrixND<2, T, ISE> inversed(const MatrixND<2, T, ISE> &mat) { |
997 | T det = determinant(mat); |
998 | return static_cast<T>(1) / det * |
999 | MatrixND<2, T, ISE>(VectorND<2, T, ISE>(mat[1][1], -mat[0][1]), |
1000 | VectorND<2, T, ISE>(-mat[1][0], mat[0][0])); |
1001 | } |
1002 | |
1003 | template <InstSetExt ISE, typename T> |
1004 | MatrixND<3, T, ISE> inversed(const MatrixND<3, T, ISE> &mat) { |
1005 | T det = determinant(mat); |
1006 | return T(1.0) / det * |
1007 | MatrixND<3, T, ISE>( |
1008 | VectorND<3, T, ISE>(mat[1][1] * mat[2][2] - mat[2][1] * mat[1][2], |
1009 | mat[2][1] * mat[0][2] - mat[0][1] * mat[2][2], |
1010 | mat[0][1] * mat[1][2] - mat[1][1] * mat[0][2]), |
1011 | VectorND<3, T, ISE>(mat[2][0] * mat[1][2] - mat[1][0] * mat[2][2], |
1012 | mat[0][0] * mat[2][2] - mat[2][0] * mat[0][2], |
1013 | mat[1][0] * mat[0][2] - mat[0][0] * mat[1][2]), |
1014 | VectorND<3, T, ISE>( |
1015 | mat[1][0] * mat[2][1] - mat[2][0] * mat[1][1], |
1016 | mat[2][0] * mat[0][1] - mat[0][0] * mat[2][1], |
1017 | mat[0][0] * mat[1][1] - mat[1][0] * mat[0][1])); |
1018 | } |
1019 | |
1020 | template <typename T, InstSetExt ISE> |
1021 | T determinant(const MatrixND<4, T, ISE> &m) { |
1022 | // This function is adopted from GLM |
1023 | /* |
1024 | ================================================================================ |
1025 | OpenGL Mathematics (GLM) |
1026 | -------------------------------------------------------------------------------- |
1027 | GLM is licensed under The Happy Bunny License and MIT License |
1028 | |
1029 | ================================================================================ |
1030 | The Happy Bunny License (Modified MIT License) |
1031 | -------------------------------------------------------------------------------- |
1032 | Copyright (c) 2005 - 2014 G-Truc Creation |
1033 | |
1034 | Permission is hereby granted, free of charge, to any person obtaining a copy |
1035 | of this software and associated documentation files (the "Software"), to deal |
1036 | in the Software without restriction, including without limitation the rights |
1037 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell |
1038 | copies of the Software, and to permit persons to whom the Software is |
1039 | furnished to do so, subject to the following conditions: |
1040 | |
1041 | The above copyright notice and this permission notice shall be included in |
1042 | all copies or substantial portions of the Software. |
1043 | |
1044 | Restrictions: |
1045 | By making use of the Software for military purposes, you choose to make a |
1046 | Bunny unhappy. |
1047 | |
1048 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR |
1049 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, |
1050 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE |
1051 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER |
1052 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, |
1053 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN |
1054 | THE SOFTWARE. |
1055 | |
1056 | ================================================================================ |
1057 | The MIT License |
1058 | -------------------------------------------------------------------------------- |
1059 | Copyright (c) 2005 - 2014 G-Truc Creation |
1060 | |
1061 | Permission is hereby granted, free of charge, to any person obtaining a copy |
1062 | of this software and associated documentation files (the "Software"), to deal |
1063 | in the Software without restriction, including without limitation the rights |
1064 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell |
1065 | copies of the Software, and to permit persons to whom the Software is |
1066 | furnished to do so, subject to the following conditions: |
1067 | |
1068 | The above copyright notice and this permission notice shall be included in |
1069 | all copies or substantial portions of the Software. |
1070 | |
1071 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR |
1072 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, |
1073 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE |
1074 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER |
1075 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, |
1076 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN |
1077 | THE SOFTWARE. |
1078 | */ |
1079 | |
1080 | T Coef00 = m[2][2] * m[3][3] - m[3][2] * m[2][3]; |
1081 | T Coef02 = m[1][2] * m[3][3] - m[3][2] * m[1][3]; |
1082 | T Coef03 = m[1][2] * m[2][3] - m[2][2] * m[1][3]; |
1083 | |
1084 | T Coef04 = m[2][1] * m[3][3] - m[3][1] * m[2][3]; |
1085 | T Coef06 = m[1][1] * m[3][3] - m[3][1] * m[1][3]; |
1086 | T Coef07 = m[1][1] * m[2][3] - m[2][1] * m[1][3]; |
1087 | |
1088 | T Coef08 = m[2][1] * m[3][2] - m[3][1] * m[2][2]; |
1089 | T Coef10 = m[1][1] * m[3][2] - m[3][1] * m[1][2]; |
1090 | T Coef11 = m[1][1] * m[2][2] - m[2][1] * m[1][2]; |
1091 | |
1092 | T Coef12 = m[2][0] * m[3][3] - m[3][0] * m[2][3]; |
1093 | T Coef14 = m[1][0] * m[3][3] - m[3][0] * m[1][3]; |
1094 | T Coef15 = m[1][0] * m[2][3] - m[2][0] * m[1][3]; |
1095 | |
1096 | T Coef16 = m[2][0] * m[3][2] - m[3][0] * m[2][2]; |
1097 | T Coef18 = m[1][0] * m[3][2] - m[3][0] * m[1][2]; |
1098 | T Coef19 = m[1][0] * m[2][2] - m[2][0] * m[1][2]; |
1099 | |
1100 | T Coef20 = m[2][0] * m[3][1] - m[3][0] * m[2][1]; |
1101 | T Coef22 = m[1][0] * m[3][1] - m[3][0] * m[1][1]; |
1102 | T Coef23 = m[1][0] * m[2][1] - m[2][0] * m[1][1]; |
1103 | |
1104 | using Vector = VectorND<4, T, ISE>; |
1105 | |
1106 | Vector Fac0(Coef00, Coef00, Coef02, Coef03); |
1107 | Vector Fac1(Coef04, Coef04, Coef06, Coef07); |
1108 | Vector Fac2(Coef08, Coef08, Coef10, Coef11); |
1109 | Vector Fac3(Coef12, Coef12, Coef14, Coef15); |
1110 | Vector Fac4(Coef16, Coef16, Coef18, Coef19); |
1111 | Vector Fac5(Coef20, Coef20, Coef22, Coef23); |
1112 | |
1113 | Vector Vec0(m[1][0], m[0][0], m[0][0], m[0][0]); |
1114 | Vector Vec1(m[1][1], m[0][1], m[0][1], m[0][1]); |
1115 | Vector Vec2(m[1][2], m[0][2], m[0][2], m[0][2]); |
1116 | Vector Vec3(m[1][3], m[0][3], m[0][3], m[0][3]); |
1117 | |
1118 | Vector Inv0(Vec1 * Fac0 - Vec2 * Fac1 + Vec3 * Fac2); |
1119 | Vector Inv1(Vec0 * Fac0 - Vec2 * Fac3 + Vec3 * Fac4); |
1120 | Vector Inv2(Vec0 * Fac1 - Vec1 * Fac3 + Vec3 * Fac5); |
1121 | Vector Inv3(Vec0 * Fac2 - Vec1 * Fac4 + Vec2 * Fac5); |
1122 | |
1123 | Vector SignA(+1, -1, +1, -1); |
1124 | Vector SignB(-1, +1, -1, +1); |
1125 | MatrixND<4, T, ISE> Inverse(Inv0 * SignA, Inv1 * SignB, Inv2 * SignA, |
1126 | Inv3 * SignB); |
1127 | |
1128 | Vector Row0(Inverse[0][0], Inverse[1][0], Inverse[2][0], Inverse[3][0]); |
1129 | |
1130 | Vector Dot0(m[0] * Row0); |
1131 | T Dot1 = (Dot0.x + Dot0.y) + (Dot0.z + Dot0.w); |
1132 | |
1133 | return Dot1; |
1134 | } |
1135 | |
1136 | template <typename T, InstSetExt ISE> |
1137 | MatrixND<4, T, ISE> inversed(const MatrixND<4, T, ISE> &m) { |
1138 | // This function is copied from GLM |
1139 | /* |
1140 | ================================================================================ |
1141 | OpenGL Mathematics (GLM) |
1142 | -------------------------------------------------------------------------------- |
1143 | GLM is licensed under The Happy Bunny License and MIT License |
1144 | |
1145 | ================================================================================ |
1146 | The Happy Bunny License (Modified MIT License) |
1147 | -------------------------------------------------------------------------------- |
1148 | Copyright (c) 2005 - 2014 G-Truc Creation |
1149 | |
1150 | Permission is hereby granted, free of charge, to any person obtaining a copy |
1151 | of this software and associated documentation files (the "Software"), to deal |
1152 | in the Software without restriction, including without limitation the rights |
1153 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell |
1154 | copies of the Software, and to permit persons to whom the Software is |
1155 | furnished to do so, subject to the following conditions: |
1156 | |
1157 | The above copyright notice and this permission notice shall be included in |
1158 | all copies or substantial portions of the Software. |
1159 | |
1160 | Restrictions: |
1161 | By making use of the Software for military purposes, you choose to make a |
1162 | Bunny unhappy. |
1163 | |
1164 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR |
1165 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, |
1166 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE |
1167 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER |
1168 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, |
1169 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN |
1170 | THE SOFTWARE. |
1171 | |
1172 | ================================================================================ |
1173 | The MIT License |
1174 | -------------------------------------------------------------------------------- |
1175 | Copyright (c) 2005 - 2014 G-Truc Creation |
1176 | |
1177 | Permission is hereby granted, free of charge, to any person obtaining a copy |
1178 | of this software and associated documentation files (the "Software"), to deal |
1179 | in the Software without restriction, including without limitation the rights |
1180 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell |
1181 | copies of the Software, and to permit persons to whom the Software is |
1182 | furnished to do so, subject to the following conditions: |
1183 | |
1184 | The above copyright notice and this permission notice shall be included in |
1185 | all copies or substantial portions of the Software. |
1186 | |
1187 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR |
1188 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, |
1189 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE |
1190 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER |
1191 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, |
1192 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN |
1193 | THE SOFTWARE. |
1194 | */ |
1195 | |
1196 | T Coef00 = m[2][2] * m[3][3] - m[3][2] * m[2][3]; |
1197 | T Coef02 = m[1][2] * m[3][3] - m[3][2] * m[1][3]; |
1198 | T Coef03 = m[1][2] * m[2][3] - m[2][2] * m[1][3]; |
1199 | |
1200 | T Coef04 = m[2][1] * m[3][3] - m[3][1] * m[2][3]; |
1201 | T Coef06 = m[1][1] * m[3][3] - m[3][1] * m[1][3]; |
1202 | T Coef07 = m[1][1] * m[2][3] - m[2][1] * m[1][3]; |
1203 | |
1204 | T Coef08 = m[2][1] * m[3][2] - m[3][1] * m[2][2]; |
1205 | T Coef10 = m[1][1] * m[3][2] - m[3][1] * m[1][2]; |
1206 | T Coef11 = m[1][1] * m[2][2] - m[2][1] * m[1][2]; |
1207 | |
1208 | T Coef12 = m[2][0] * m[3][3] - m[3][0] * m[2][3]; |
1209 | T Coef14 = m[1][0] * m[3][3] - m[3][0] * m[1][3]; |
1210 | T Coef15 = m[1][0] * m[2][3] - m[2][0] * m[1][3]; |
1211 | |
1212 | T Coef16 = m[2][0] * m[3][2] - m[3][0] * m[2][2]; |
1213 | T Coef18 = m[1][0] * m[3][2] - m[3][0] * m[1][2]; |
1214 | T Coef19 = m[1][0] * m[2][2] - m[2][0] * m[1][2]; |
1215 | |
1216 | T Coef20 = m[2][0] * m[3][1] - m[3][0] * m[2][1]; |
1217 | T Coef22 = m[1][0] * m[3][1] - m[3][0] * m[1][1]; |
1218 | T Coef23 = m[1][0] * m[2][1] - m[2][0] * m[1][1]; |
1219 | |
1220 | using Vector = VectorND<4, T, ISE>; |
1221 | |
1222 | Vector Fac0(Coef00, Coef00, Coef02, Coef03); |
1223 | Vector Fac1(Coef04, Coef04, Coef06, Coef07); |
1224 | Vector Fac2(Coef08, Coef08, Coef10, Coef11); |
1225 | Vector Fac3(Coef12, Coef12, Coef14, Coef15); |
1226 | Vector Fac4(Coef16, Coef16, Coef18, Coef19); |
1227 | Vector Fac5(Coef20, Coef20, Coef22, Coef23); |
1228 | |
1229 | Vector Vec0(m[1][0], m[0][0], m[0][0], m[0][0]); |
1230 | Vector Vec1(m[1][1], m[0][1], m[0][1], m[0][1]); |
1231 | Vector Vec2(m[1][2], m[0][2], m[0][2], m[0][2]); |
1232 | Vector Vec3(m[1][3], m[0][3], m[0][3], m[0][3]); |
1233 | |
1234 | Vector Inv0(Vec1 * Fac0 - Vec2 * Fac1 + Vec3 * Fac2); |
1235 | Vector Inv1(Vec0 * Fac0 - Vec2 * Fac3 + Vec3 * Fac4); |
1236 | Vector Inv2(Vec0 * Fac1 - Vec1 * Fac3 + Vec3 * Fac5); |
1237 | Vector Inv3(Vec0 * Fac2 - Vec1 * Fac4 + Vec2 * Fac5); |
1238 | |
1239 | Vector SignA(+1, -1, +1, -1); |
1240 | Vector SignB(-1, +1, -1, +1); |
1241 | MatrixND<4, T, ISE> Inverse(Inv0 * SignA, Inv1 * SignB, Inv2 * SignA, |
1242 | Inv3 * SignB); |
1243 | |
1244 | Vector Row0(Inverse[0][0], Inverse[1][0], Inverse[2][0], Inverse[3][0]); |
1245 | |
1246 | Vector Dot0(m[0] * Row0); |
1247 | T Dot1 = (Dot0.x + Dot0.y) + (Dot0.z + Dot0.w); |
1248 | |
1249 | T OneOverDeterminant = static_cast<T>(1) / Dot1; |
1250 | |
1251 | return Inverse * OneOverDeterminant; |
1252 | } |
1253 | |
1254 | template <int dim, typename T, InstSetExt ISE> |
1255 | TI_FORCE_INLINE MatrixND<dim, T, ISE> inverse(const MatrixND<dim, T, ISE> &m) { |
1256 | return inversed(m); |
1257 | } |
1258 | |
1259 | TI_FORCE_INLINE Vector3 multiply_matrix4(const Matrix4 &m, |
1260 | const Vector3 &v, |
1261 | real w) { |
1262 | return Vector3(m * Vector4(v, w)); |
1263 | } |
1264 | |
1265 | template <int dim> |
1266 | TI_FORCE_INLINE VectorND<dim, real> transform(const MatrixND<dim + 1, real> &m, |
1267 | const VectorND<dim, real> &v, |
1268 | real w = 1.0_f) { |
1269 | return VectorND<dim, real>(m * VectorND<dim + 1, real>(v, w)); |
1270 | } |
1271 | |
1272 | // Type traits |
1273 | |
1274 | template <typename T> |
1275 | struct is_vector { |
1276 | static constexpr bool value = false; |
1277 | }; |
1278 | |
1279 | template <int dim, typename T, InstSetExt ISE> |
1280 | struct is_vector<VectorND<dim, T, ISE>> { |
1281 | static constexpr bool value = true; |
1282 | }; |
1283 | |
1284 | template <typename T> |
1285 | struct is_matrix { |
1286 | static constexpr bool value = false; |
1287 | }; |
1288 | |
1289 | template <int dim, typename T, InstSetExt ISE> |
1290 | struct is_matrix<MatrixND<dim, T, ISE>> { |
1291 | static constexpr bool value = true; |
1292 | }; |
1293 | |
1294 | template <int dim, typename T> |
1295 | TI_FORCE_INLINE VectorND<dim, T> min(const VectorND<dim, T> &a, |
1296 | const VectorND<dim, T> &b) { |
1297 | VectorND<dim, T> ret; |
1298 | for (int i = 0; i < dim; i++) { |
1299 | ret[i] = std::min(a[i], b[i]); |
1300 | } |
1301 | return ret; |
1302 | } |
1303 | |
1304 | template <int dim, typename T> |
1305 | TI_FORCE_INLINE VectorND<dim, T> max(const VectorND<dim, T> &a, |
1306 | const VectorND<dim, T> &b) { |
1307 | VectorND<dim, T> ret; |
1308 | for (int i = 0; i < dim; i++) { |
1309 | ret[i] = std::max(a[i], b[i]); |
1310 | } |
1311 | return ret; |
1312 | } |
1313 | |
1314 | inline Matrix4 matrix4_translate(Matrix4 *transform, const Vector3 &offset) { |
1315 | return Matrix4(Vector4(1, 0, 0, 0), Vector4(0, 1, 0, 0), Vector4(0, 0, 1, 0), |
1316 | Vector4(offset, 1.0_f)) * |
1317 | *transform; |
1318 | } |
1319 | |
1320 | inline Matrix4 matrix_translate(Matrix4 *transform, const Vector3 &offset) { |
1321 | return matrix4_translate(transform, offset); |
1322 | } |
1323 | |
1324 | inline Matrix3 matrix_translate(Matrix3 *transform, const Vector2 &offset) { |
1325 | return Matrix3(Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(offset, 1.0_f)) * |
1326 | *transform; |
1327 | } |
1328 | |
1329 | inline Matrix3 matrix_scale(Matrix3 *transform, const Vector2 &scales) { |
1330 | return Matrix3(Vector3(scales, 1.0_f)) * *transform; |
1331 | } |
1332 | |
1333 | inline Matrix4 matrix4_scale(Matrix4 *transform, const Vector3 &scales) { |
1334 | return Matrix4(Vector4(scales, 1.0_f)) * *transform; |
1335 | } |
1336 | |
1337 | inline Matrix4 matrix_scale(Matrix4 *transform, const Vector3 &scales) { |
1338 | return Matrix4(Vector4(scales, 1.0_f)) * *transform; |
1339 | } |
1340 | |
1341 | inline Matrix4 matrix4_scale_s(Matrix4 *transform, real s) { |
1342 | return matrix4_scale(transform, Vector3(s)); |
1343 | } |
1344 | |
1345 | // Reference: https://en.wikipedia.org/wiki/Rotation_matrix |
1346 | inline Matrix4 get_rotation_matrix(Vector3 u, real angle) { |
1347 | u = normalized(u); |
1348 | real c = cos(angle), s = sin(angle); |
1349 | real d = 1 - c; |
1350 | |
1351 | auto col0 = Vector4(c + u.x * u.x * d, u.x * u.y * d - u.z * s, |
1352 | u.x * u.z * d + u.y * s, 0.0_f); |
1353 | auto col1 = Vector4(u.x * u.y * d + u.z * s, c + u.y * u.y * d, |
1354 | u.y * u.z * d - u.x * s, 0.0_f); |
1355 | auto col2 = Vector4(u.x * u.z * d - u.y * s, u.y * u.z * d + u.x * s, |
1356 | c + u.z * u.z * d, 0.0_f); |
1357 | auto col3 = Vector4(0.0_f, 0.0_f, 0.0_f, 1.0_f); |
1358 | |
1359 | return Matrix4(col0, col1, col2, col3).transposed(); |
1360 | } |
1361 | |
1362 | inline Matrix4 matrix4_rotate_angle_axis(Matrix4 *transform, |
1363 | real angle, |
1364 | const Vector3 &axis) { |
1365 | return get_rotation_matrix(axis, angle * (pi / 180.0_f)) * *transform; |
1366 | } |
1367 | |
1368 | inline Matrix4 matrix4_rotate_euler(Matrix4 *transform, |
1369 | const Vector3 &euler_angles) { |
1370 | Matrix4 ret = *transform; |
1371 | ret = matrix4_rotate_angle_axis(&ret, euler_angles.x, |
1372 | Vector3(1.0_f, 0.0_f, 0.0_f)); |
1373 | ret = matrix4_rotate_angle_axis(&ret, euler_angles.y, |
1374 | Vector3(0.0_f, 1.0_f, 0.0_f)); |
1375 | ret = matrix4_rotate_angle_axis(&ret, euler_angles.z, |
1376 | Vector3(0.0_f, 0.0_f, 1.0_f)); |
1377 | return ret; |
1378 | } |
1379 | |
1380 | template <typename T> |
1381 | inline MatrixND<3, T> cross_product_matrix(const VectorND<3, T> &a) { |
1382 | return MatrixND<3, T>(VectorND<3, T>(0, a[2], -a[1]), |
1383 | VectorND<3, T>(-a[2], 0, a[0]), |
1384 | VectorND<3, T>(a[1], -a[0], 0)); |
1385 | }; |
1386 | |
1387 | static_assert(Serializer::has_io<Matrix4>::value, "" ); |
1388 | static_assert(Serializer::has_io<const Matrix4>::value, "" ); |
1389 | static_assert(Serializer::has_io<const Matrix4 &>::value, "" ); |
1390 | static_assert(Serializer::has_io<Matrix4 &>::value, "" ); |
1391 | |
1392 | namespace type { |
1393 | template <typename T, typename = void> |
1394 | struct element_; |
1395 | |
1396 | template <typename T> |
1397 | struct element_<T, typename std::enable_if_t<std::is_arithmetic<T>::value>> { |
1398 | using type = T; |
1399 | }; |
1400 | |
1401 | template <typename T> |
1402 | struct element_<T, typename std::enable_if_t<!std::is_arithmetic<T>::value>> { |
1403 | using type = typename T::ScalarType; |
1404 | }; |
1405 | |
1406 | template <typename T> |
1407 | using element = typename element_<std::decay_t<T>>::type; |
1408 | |
1409 | template <typename> |
1410 | struct is_VectorND : public std::false_type {}; |
1411 | |
1412 | template <int N, typename T, InstSetExt ISE> |
1413 | struct is_VectorND<VectorND<N, T, ISE>> : public std::true_type {}; |
1414 | |
1415 | template <typename> |
1416 | struct is_MatrixND : public std::false_type {}; |
1417 | |
1418 | template <int N, typename T, InstSetExt ISE> |
1419 | struct is_MatrixND<MatrixND<N, T, ISE>> : public std::true_type {}; |
1420 | } // namespace type |
1421 | |
1422 | } // namespace taichi |
1423 | |