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"
16namespace taichi {
17
18// Instruction Set Extension
19
20enum class InstSetExt { None };
21
22constexpr InstSetExt default_instruction_set = InstSetExt::None;
23
24/////////////////////////////////////////////////////////////////
25///// N dimensional Vector
26/////////////////////////////////////////////////////////////////
27
28template <int dim, typename T, InstSetExt ISE>
29struct VectorNDBase {
30 static constexpr bool simd = false;
31 static constexpr int storage_elements = dim;
32 T d[dim];
33};
34
35template <typename T, InstSetExt ISE>
36struct 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
47template <typename T, InstSetExt ISE>
48struct 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
59template <typename T, InstSetExt ISE>
60struct 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
71template <typename T, InstSetExt ISE>
72struct 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
83template <int dim__, typename T, InstSetExt ISE = default_instruction_set>
84struct 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 extra) {
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
524template <typename T, int dim, InstSetExt ISE = default_instruction_set>
525using TVector = VectorND<dim, T, ISE>;
526
527template <int dim, typename T, InstSetExt ISE>
528TI_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
534template <int dim, typename T, InstSetExt ISE>
535TI_FORCE_INLINE VectorND<dim, T, ISE> operator*(const VectorND<dim, T, ISE> &v,
536 T a) {
537 return a * v;
538}
539
540template <int dim, typename T, InstSetExt ISE>
541TI_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
547template <int dim, typename T, InstSetExt ISE>
548TI_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
553template <typename T>
554TI_FORCE_INLINE std::array<T, 1> to_std_array(const TVector<T, 1> &v) {
555 return std::array<T, 1>{v[0]};
556}
557
558template <typename T>
559TI_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
563template <typename T>
564TI_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
568template <typename T>
569TI_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
573using Vector1 = VectorND<1, real, default_instruction_set>;
574using Vector2 = VectorND<2, real, default_instruction_set>;
575using Vector3 = VectorND<3, real, default_instruction_set>;
576using Vector4 = VectorND<4, real, default_instruction_set>;
577
578using Vector1f = VectorND<1, float32, default_instruction_set>;
579using Vector2f = VectorND<2, float32, default_instruction_set>;
580using Vector3f = VectorND<3, float32, default_instruction_set>;
581using Vector4f = VectorND<4, float32, default_instruction_set>;
582
583using Vector1d = VectorND<1, float64, default_instruction_set>;
584using Vector2d = VectorND<2, float64, default_instruction_set>;
585using Vector3d = VectorND<3, float64, default_instruction_set>;
586using Vector4d = VectorND<4, float64, default_instruction_set>;
587
588using Vector1i = VectorND<1, int, default_instruction_set>;
589using Vector2i = VectorND<2, int, default_instruction_set>;
590using Vector3i = VectorND<3, int, default_instruction_set>;
591using Vector4i = VectorND<4, int, default_instruction_set>;
592
593template <typename T>
594TI_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
602template <int dim__, typename T, InstSetExt ISE = default_instruction_set>
603struct 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
869template <int dim, typename T, InstSetExt ISE>
870TI_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
880template <int dim, typename T, InstSetExt ISE>
881TI_FORCE_INLINE MatrixND<dim, T, ISE> operator*(const MatrixND<dim, T, ISE> &M,
882 const T a) {
883 return a * M;
884}
885
886template <int dim, typename T, InstSetExt ISE>
887TI_FORCE_INLINE MatrixND<dim, T, ISE> transpose(
888 const MatrixND<dim, T, ISE> &mat) {
889 return mat.transposed();
890}
891
892template <int dim, typename T, InstSetExt ISE>
893TI_FORCE_INLINE MatrixND<dim, T, ISE> transposed(
894 const MatrixND<dim, T, ISE> &mat) {
895 return transpose(mat);
896}
897
898template <typename T, int dim, InstSetExt ISE = default_instruction_set>
899using TMatrix = MatrixND<dim, T, ISE>;
900
901using Matrix2 = MatrixND<2, real, default_instruction_set>;
902using Matrix3 = MatrixND<3, real, default_instruction_set>;
903using Matrix4 = MatrixND<4, real, default_instruction_set>;
904
905using Matrix2f = MatrixND<2, float32, default_instruction_set>;
906using Matrix3f = MatrixND<3, float32, default_instruction_set>;
907using Matrix4f = MatrixND<4, float32, default_instruction_set>;
908
909using Matrix2d = MatrixND<2, float64, default_instruction_set>;
910using Matrix3d = MatrixND<3, float64, default_instruction_set>;
911using Matrix4d = MatrixND<4, float64, default_instruction_set>;
912
913template <typename T, InstSetExt ISE>
914TI_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
918template <typename T, InstSetExt ISE>
919TI_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
925template <typename T, InstSetExt ISE>
926TI_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
931template <typename T, InstSetExt ISE>
932TI_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
938template <int dim, typename T, InstSetExt ISE>
939TI_FORCE_INLINE T dot(const VectorND<dim, T, ISE> &a,
940 const VectorND<dim, T, ISE> &b) {
941 return a.dot(b);
942}
943
944template <int dim, typename T, InstSetExt ISE>
945TI_FORCE_INLINE VectorND<dim, T, ISE> normalize(
946 const VectorND<dim, T, ISE> &a) {
947 return (T(1) / a.length()) * a;
948}
949
950template <int dim, typename T, InstSetExt ISE>
951TI_FORCE_INLINE VectorND<dim, T, ISE> normalized(
952 const VectorND<dim, T, ISE> &a) {
953 return normalize(a);
954}
955
956TI_FORCE_INLINE float32 length(const float32 &a) {
957 return a;
958}
959
960TI_FORCE_INLINE float64 length(const float64 &a) {
961 return a;
962}
963
964template <int dim, typename T, InstSetExt ISE>
965TI_FORCE_INLINE T length(const VectorND<dim, T, ISE> &a) {
966 return a.length();
967}
968
969template <int dim, typename T, InstSetExt ISE>
970TI_FORCE_INLINE T length2(const VectorND<dim, T, ISE> &a) {
971 return dot(a, a);
972}
973
974TI_FORCE_INLINE float32 length2(const float32 &a) {
975 return a * a;
976}
977
978TI_FORCE_INLINE float64 length2(const float64 &a) {
979 return a * a;
980}
981
982template <int dim, typename T, InstSetExt ISE>
983TI_FORCE_INLINE VectorND<dim, T, ISE> fract(const VectorND<dim, T, ISE> &a) {
984 return a.fract();
985}
986
987TI_FORCE_INLINE float32 inversed(const float32 &a) {
988 return 1.0_f32 / a;
989}
990
991TI_FORCE_INLINE float64 inversed(const float64 &a) {
992 return 1.0_f64 / a;
993}
994
995template <InstSetExt ISE, typename T>
996TI_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
1003template <InstSetExt ISE, typename T>
1004MatrixND<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
1020template <typename T, InstSetExt ISE>
1021T 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
1136template <typename T, InstSetExt ISE>
1137MatrixND<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
1254template <int dim, typename T, InstSetExt ISE>
1255TI_FORCE_INLINE MatrixND<dim, T, ISE> inverse(const MatrixND<dim, T, ISE> &m) {
1256 return inversed(m);
1257}
1258
1259TI_FORCE_INLINE Vector3 multiply_matrix4(const Matrix4 &m,
1260 const Vector3 &v,
1261 real w) {
1262 return Vector3(m * Vector4(v, w));
1263}
1264
1265template <int dim>
1266TI_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
1274template <typename T>
1275struct is_vector {
1276 static constexpr bool value = false;
1277};
1278
1279template <int dim, typename T, InstSetExt ISE>
1280struct is_vector<VectorND<dim, T, ISE>> {
1281 static constexpr bool value = true;
1282};
1283
1284template <typename T>
1285struct is_matrix {
1286 static constexpr bool value = false;
1287};
1288
1289template <int dim, typename T, InstSetExt ISE>
1290struct is_matrix<MatrixND<dim, T, ISE>> {
1291 static constexpr bool value = true;
1292};
1293
1294template <int dim, typename T>
1295TI_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
1304template <int dim, typename T>
1305TI_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
1314inline 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
1320inline Matrix4 matrix_translate(Matrix4 *transform, const Vector3 &offset) {
1321 return matrix4_translate(transform, offset);
1322}
1323
1324inline 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
1329inline Matrix3 matrix_scale(Matrix3 *transform, const Vector2 &scales) {
1330 return Matrix3(Vector3(scales, 1.0_f)) * *transform;
1331}
1332
1333inline Matrix4 matrix4_scale(Matrix4 *transform, const Vector3 &scales) {
1334 return Matrix4(Vector4(scales, 1.0_f)) * *transform;
1335}
1336
1337inline Matrix4 matrix_scale(Matrix4 *transform, const Vector3 &scales) {
1338 return Matrix4(Vector4(scales, 1.0_f)) * *transform;
1339}
1340
1341inline 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
1346inline 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
1362inline 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
1368inline 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
1380template <typename T>
1381inline 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
1387static_assert(Serializer::has_io<Matrix4>::value, "");
1388static_assert(Serializer::has_io<const Matrix4>::value, "");
1389static_assert(Serializer::has_io<const Matrix4 &>::value, "");
1390static_assert(Serializer::has_io<Matrix4 &>::value, "");
1391
1392namespace type {
1393template <typename T, typename = void>
1394struct element_;
1395
1396template <typename T>
1397struct element_<T, typename std::enable_if_t<std::is_arithmetic<T>::value>> {
1398 using type = T;
1399};
1400
1401template <typename T>
1402struct element_<T, typename std::enable_if_t<!std::is_arithmetic<T>::value>> {
1403 using type = typename T::ScalarType;
1404};
1405
1406template <typename T>
1407using element = typename element_<std::decay_t<T>>::type;
1408
1409template <typename>
1410struct is_VectorND : public std::false_type {};
1411
1412template <int N, typename T, InstSetExt ISE>
1413struct is_VectorND<VectorND<N, T, ISE>> : public std::true_type {};
1414
1415template <typename>
1416struct is_MatrixND : public std::false_type {};
1417
1418template <int N, typename T, InstSetExt ISE>
1419struct is_MatrixND<MatrixND<N, T, ISE>> : public std::true_type {};
1420} // namespace type
1421
1422} // namespace taichi
1423